Mercurial > deltabot
changeset 0:5036b8bfe26b draft default tip
Initial commit
| author | Radomir Dopieralski <sheep-devel@sheep.art.pl> |
|---|---|
| date | Sat, 18 Mar 2017 21:50:54 +0100 |
| parents | |
| children | |
| files | pca9685.py robot.py servo.py |
| diffstat | 3 files changed, 157 insertions(+), 0 deletions(-) [+] |
line wrap: on
line diff
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/pca9685.py Sat Mar 18 21:50:54 2017 +0100 @@ -0,0 +1,59 @@ +import ustruct +import time + + +class PCA9685: + def __init__(self, i2c, address=0x40): + self.i2c = i2c + self.address = address + self.reset() + + def _write(self, address, value): + self.i2c.writeto_mem(self.address, address, bytearray([value])) + + def _read(self, address): + return self.i2c.readfrom_mem(self.address, address, 1)[0] + + def reset(self): + self._write(0x00, 0x00) # Mode1 + + def freq(self, freq=None): + if freq is None: + return int(25000000.0 / 4096 / (self._read(0xfe) - 0.5)) + prescale = int(25000000.0 / 4096.0 / freq + 0.5) + old_mode = self._read(0x00) # Mode 1 + self._write(0x00, (old_mode & 0x7F) | 0x10) # Mode 1, sleep + self._write(0xfe, prescale) # Prescale + self._write(0x00, old_mode) # Mode 1 + time.sleep_us(5) + self._write(0x00, old_mode | 0xa1) # Mode 1, autoincrement on + + def pwm(self, index, on=None, off=None): + if on is None or off is None: + data = self.i2c.readfrom_mem(self.address, 0x06 + 4 * index, 4) + return ustruct.unpack('<HH', data) + data = ustruct.pack('<HH', on, off) + self.i2c.writeto_mem(self.address, 0x06 + 4 * index, data) + + def duty(self, index, value=None, invert=False): + if value is None: + pwm = self.pwm(index) + if pwm == (0, 4096): + value = 0 + elif pwm == (4096, 0): + value = 4095 + value = pwm[1] + if invert: + value = 4095 - value + return value + if not 0 <= value <= 4095: + raise ValueError("Out of range") + if invert: + value = 4095 - value + if value == 0: + self.pwm(index, 0, 4096) + elif value == 4095: + self.pwm(index, 4096, 0) + else: + self.pwm(index, 0, value) +
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/robot.py Sat Mar 18 21:50:54 2017 +0100 @@ -0,0 +1,63 @@ +import math +from machine import I2C, Pin +import servo + + +PI2 = math.pi / 2 +PI3 = math.pi / 3 + + +class Robot: + ARM_LENGTH = 42 + ROD_LENGTH = 134 + HUB_LENGTH = 28 + BASE_LENGTH = 64 + TMP_LENGTH = BASE_LENGTH - HUB_LENGTH # pretend that hub has size 0 + ARM2ROD2 = ARM_LENGTH ** 2 - ROD_LENGTH ** 2 + + BASE_X = ( + 0, + TMP_LENGTH * math.sin(2 * PI3), + TMP_LENGTH * math.sin(4 * PI3), + ) + BASE_Y = ( + TMP_LENGTH, + TMP_LENGTH * math.cos(2 * PI3), + TMP_LENGTH * math.cos(4 * PI3), + ) + + def __init__(self): + self.i2c = I2C(-1, Pin(5), Pin(4)) + self.servos = servo.Servos(self.i2c, min_us=700, max_us=2400, + degrees=180) + self.trims = (-0.25, -0.28, -0.4) + + def home(self): + self.move(0, 0) + self.move(1, 0) + self.move(2, 0) + + def move(self, arm, radians): + radians += self.trims[arm] + PI2 + self.servos.position(arm, radians=radians) + + def arm_ik(self, distance, z): + alpha = math.asin(z / distance) + beta = math.acos( + (distance ** 2 + self.ARM2ROD2) / (2 * self.ARM_LENGTH * distance) + ) + return math.pi - alpha - beta + + def robot_ik(self, x, y, z): + return tuple(math.sqrt( + (self.BASE_X[arm] - x) ** 2 + + (self.BASE_Y[arm] - y) ** 2 + + z ** 2 + ) for arm in (0, 1, 2)) + + def move_to(self, x, y, z): + for arm, distance in enumerate(self.robot_ik(x, y, z)): + self.move(arm, self.arm_ik(distance, z)) + + +robot = Robot()
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/servo.py Sat Mar 18 21:50:54 2017 +0100 @@ -0,0 +1,35 @@ +import pca9685 +import math + + +class Servos: + def __init__(self, i2c, address=0x40, freq=50, min_us=600, max_us=2400, + degrees=180): + self.period = 1000000 / freq + self.min_duty = self._us2duty(min_us) + self.max_duty = self._us2duty(max_us) + self.degrees = degrees + self.freq = freq + self.pca9685 = pca9685.PCA9685(i2c, address) + self.pca9685.freq(freq) + + def _us2duty(self, value): + return int(4095 * value / self.period) + + def position(self, index, degrees=None, radians=None, us=None, duty=None): + span = self.max_duty - self.min_duty + if degrees is not None: + duty = self.min_duty + span * degrees / self.degrees + elif radians is not None: + duty = self.min_duty + span * radians / math.radians(self.degrees) + elif us is not None: + duty = self._us2duty(us) + elif duty is not None: + pass + else: + return self.pca9685.duty(index) + duty = min(self.max_duty, max(self.min_duty, int(duty))) + self.pca9685.duty(index, duty) + + def release(self, index): + self.pca9685.duty(index, 0)