Adding motors and updating scripts
Drew Bednar
5 years ago
parent
5ffdc6e63c
commit
3a26cf4b16
@ -0,0 +1,13 @@
|
||||
# Motors (servos, steppers, DC)
|
||||
|
||||
The modules for this came from this legacy Adafruit repo https://github.com/adafruit/micropython-adafruit-pca9685. Look at it's
|
||||
[Docs](https://micropython-pca9685.readthedocs.io/en/latest/)
|
||||
|
||||
## I2C Servo driver (PCA9685)
|
||||
|
||||
Using the [PCA9685](https://www.adafruit.com/product/815) from adafruit, we can drive multiple
|
||||
servos using PWM. A large amount of the content for this read my comes from the adafruit article
|
||||
on [PCA9685 with Micropython](https://learn.adafruit.com/micropython-hardware-pca9685-pwm-and-servo-driver/micropython)
|
||||
|
||||
The git repo https://github.com/adafruit/micropython-adafruit-pca9685 is still up. Look at it's
|
||||
[Docs](https://micropython-pca9685.readthedocs.io/en/latest/)
|
||||
@ -0,0 +1,45 @@
|
||||
import pca9685
|
||||
|
||||
|
||||
_DC_MOTORS = ((8, 9, 10), (13, 12, 11), (2, 3, 4), (7, 6, 5))
|
||||
|
||||
|
||||
class DCMotors:
|
||||
def __init__(self, i2c, address=0x60, freq=1600):
|
||||
self.pca9685 = pca9685.PCA9685(i2c, address)
|
||||
self.pca9685.freq(freq)
|
||||
|
||||
def _pin(self, pin, value=None):
|
||||
if value is None:
|
||||
return bool(self.pca9685.pwm(pin)[0])
|
||||
if value:
|
||||
self.pca9685.pwm(pin, 4096, 0)
|
||||
else:
|
||||
self.pca9685.pwm(pin, 0, 0)
|
||||
|
||||
def speed(self, index, value=None):
|
||||
pwm, in2, in1 = _DC_MOTORS[index]
|
||||
if value is None:
|
||||
value = self.pca9685.duty(pwm)
|
||||
if self._pin(in2) and not self._pin(in1):
|
||||
value = -value
|
||||
return value
|
||||
if value > 0:
|
||||
# Forward
|
||||
self._pin(in2, False)
|
||||
self._pin(in1, True)
|
||||
elif value < 0:
|
||||
# Backward
|
||||
self._pin(in1, False)
|
||||
self._pin(in2, True)
|
||||
else:
|
||||
# Release
|
||||
self._pin(in1, False)
|
||||
self._pin(in2, False)
|
||||
self.pca9685.duty(pwm, abs(value))
|
||||
|
||||
def brake(self, index):
|
||||
pwm, in2, in1 = _DC_MOTORS[index]
|
||||
self._pin(in1, True)
|
||||
self._pin(in2, True)
|
||||
self.pca9685.duty(pwm, 0)
|
||||
@ -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)
|
||||
|
||||
@ -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)
|
||||
@ -0,0 +1,170 @@
|
||||
# Stepper Motor Shield/Wing Driver
|
||||
# Based on Adafruit Motorshield library:
|
||||
# https://github.com/adafruit/Adafruit_Motor_Shield_V2_Library
|
||||
# Author: Tony DiCola
|
||||
import pca9685
|
||||
|
||||
|
||||
# Constants that specify the direction and style of steps.
|
||||
FORWARD = const(1)
|
||||
BACKWARD = const(2)
|
||||
SINGLE = const(1)
|
||||
DOUBLE = const(2)
|
||||
INTERLEAVE = const(3)
|
||||
MICROSTEP = const(4)
|
||||
|
||||
# Not a const so users can change this global to 8 or 16 to change step size
|
||||
MICROSTEPS = 16
|
||||
|
||||
# Microstepping curves (these are constants but need to be tuples/indexable):
|
||||
_MICROSTEPCURVE8 = (0, 50, 98, 142, 180, 212, 236, 250, 255)
|
||||
_MICROSTEPCURVE16 = (0, 25, 50, 74, 98, 120, 141, 162, 180, 197, 212, 225, 236, 244, 250, 253, 255)
|
||||
|
||||
# Define PWM outputs for each of two available steppers.
|
||||
# Each tuple defines for a stepper: pwma, ain2, ain1, pwmb, bin2, bin1
|
||||
_STEPPERS = ((8, 9, 10, 13, 12, 11), (2, 3, 4, 7, 6, 5))
|
||||
|
||||
|
||||
class StepperMotor:
|
||||
def __init__(self, pca, pwma, ain2, ain1, pwmb, bin2, bin1):
|
||||
self.pca9685 = pca
|
||||
self.pwma = pwma
|
||||
self.ain2 = ain2
|
||||
self.ain1 = ain1
|
||||
self.pwmb = pwmb
|
||||
self.bin2 = bin2
|
||||
self.bin1 = bin1
|
||||
self.currentstep = 0
|
||||
|
||||
def _pwm(self, pin, value):
|
||||
if value > 4095:
|
||||
self.pca9685.pwm(pin, 4096, 0)
|
||||
else:
|
||||
self.pca9685.pwm(pin, 0, value)
|
||||
|
||||
def _pin(self, pin, value):
|
||||
if value:
|
||||
self.pca9685.pwm(pin, 4096, 0)
|
||||
else:
|
||||
self.pca9685.pwm(pin, 0, 0)
|
||||
|
||||
def onestep(self, direction, style):
|
||||
ocra = 255
|
||||
ocrb = 255
|
||||
# Adjust current steps based on the direction and type of step.
|
||||
if style == SINGLE:
|
||||
if (self.currentstep//(MICROSTEPS//2)) % 2:
|
||||
if direction == FORWARD:
|
||||
self.currentstep += MICROSTEPS//2
|
||||
else:
|
||||
self.currentstep -= MICROSTEPS//2
|
||||
else:
|
||||
if direction == FORWARD:
|
||||
self.currentstep += MICROSTEPS
|
||||
else:
|
||||
self.currentstep -= MICROSTEPS
|
||||
elif style == DOUBLE:
|
||||
if not (self.currentstep//(MICROSTEPS//2)) % 2:
|
||||
if direction == FORWARD:
|
||||
self.currentstep += MICROSTEPS//2
|
||||
else:
|
||||
self.currentstep -= MICROSTEPS//2
|
||||
else:
|
||||
if direction == FORWARD:
|
||||
self.currentstep += MICROSTEPS
|
||||
else:
|
||||
self.currentstep -= MICROSTEPS
|
||||
elif style == INTERLEAVE:
|
||||
if direction == FORWARD:
|
||||
self.currentstep += MICROSTEPS//2
|
||||
else:
|
||||
self.currentstep -= MICROSTEPS//2
|
||||
elif style == MICROSTEP:
|
||||
if direction == FORWARD:
|
||||
self.currentstep += 1
|
||||
else:
|
||||
self.currentstep -= 1
|
||||
self.currentstep += MICROSTEPS*4
|
||||
self.currentstep %= MICROSTEPS*4
|
||||
ocra = 0
|
||||
ocrb = 0
|
||||
if MICROSTEPS == 8:
|
||||
curve = _MICROSTEPCURVE8
|
||||
elif MICROSTEPS == 16:
|
||||
curve = _MICROSTEPCURVE16
|
||||
else:
|
||||
raise RuntimeError('MICROSTEPS must be 8 or 16!')
|
||||
if 0 <= self.currentstep < MICROSTEPS:
|
||||
ocra = curve[MICROSTEPS - self.currentstep]
|
||||
ocrb = curve[self.currentstep]
|
||||
elif MICROSTEPS <= self.currentstep < MICROSTEPS*2:
|
||||
ocra = curve[self.currentstep - MICROSTEPS]
|
||||
ocrb = curve[MICROSTEPS*2 - self.currentstep]
|
||||
elif MICROSTEPS*2 <= self.currentstep < MICROSTEPS*3:
|
||||
ocra = curve[MICROSTEPS*3 - self.currentstep]
|
||||
ocrb = curve[self.currentstep - MICROSTEPS*2]
|
||||
elif MICROSTEPS*3 <= self.currentstep < MICROSTEPS*4:
|
||||
ocra = curve[self.currentstep - MICROSTEPS*3]
|
||||
ocrb = curve[MICROSTEPS*4 - self.currentstep]
|
||||
self.currentstep += MICROSTEPS*4
|
||||
self.currentstep %= MICROSTEPS*4
|
||||
# Set PWM outputs.
|
||||
self._pwm(self.pwma, ocra*16)
|
||||
self._pwm(self.pwmb, ocrb*16)
|
||||
latch_state = 0
|
||||
# Determine which coils to energize:
|
||||
if style == MICROSTEP:
|
||||
if 0 <= self.currentstep < MICROSTEPS:
|
||||
latch_state |= 0x3
|
||||
elif MICROSTEPS <= self.currentstep < MICROSTEPS*2:
|
||||
latch_state |= 0x6
|
||||
elif MICROSTEPS*2 <= self.currentstep < MICROSTEPS*3:
|
||||
latch_state |= 0xC
|
||||
elif MICROSTEPS*3 <= self.currentstep < MICROSTEPS*4:
|
||||
latch_state |= 0x9
|
||||
else:
|
||||
latch_step = self.currentstep//(MICROSTEPS//2)
|
||||
if latch_step == 0:
|
||||
latch_state |= 0x1 # energize coil 1 only
|
||||
elif latch_step == 1:
|
||||
latch_state |= 0x3 # energize coil 1+2
|
||||
elif latch_step == 2:
|
||||
latch_state |= 0x2 # energize coil 2 only
|
||||
elif latch_step == 3:
|
||||
latch_state |= 0x6 # energize coil 2+3
|
||||
elif latch_step == 4:
|
||||
latch_state |= 0x4 # energize coil 3 only
|
||||
elif latch_step == 5:
|
||||
latch_state |= 0xC # energize coil 3+4
|
||||
elif latch_step == 6:
|
||||
latch_state |= 0x8 # energize coil 4 only
|
||||
elif latch_step == 7:
|
||||
latch_state |= 0x9 # energize coil 1+4
|
||||
# Energize coils as appropriate:
|
||||
if latch_state & 0x1:
|
||||
self._pin(self.ain2, True)
|
||||
else:
|
||||
self._pin(self.ain2, False)
|
||||
if latch_state & 0x2:
|
||||
self._pin(self.bin1, True)
|
||||
else:
|
||||
self._pin(self.bin1, False)
|
||||
if latch_state & 0x4:
|
||||
self._pin(self.ain1, True)
|
||||
else:
|
||||
self._pin(self.ain1, False)
|
||||
if latch_state & 0x8:
|
||||
self._pin(self.bin2, True)
|
||||
else:
|
||||
self._pin(self.bin2, False)
|
||||
return self.currentstep
|
||||
|
||||
|
||||
class Steppers:
|
||||
def __init__(self, i2c, address=0x60, freq=1600):
|
||||
self.pca9685 = pca9685.PCA9685(i2c, address)
|
||||
self.pca9685.freq(freq)
|
||||
|
||||
def get_stepper(self, num):
|
||||
pwma, ain2, ain1, pwmb, bin2, bin1 = _STEPPERS[num]
|
||||
return StepperMotor(self.pca9685, pwma, ain2, ain1, pwmb, bin2, bin1)
|
||||
@ -0,0 +1,14 @@
|
||||
export AMPY_PORT=/dev/cu.SLAB_USBtoUART
|
||||
export BAUD_RATE=115200
|
||||
|
||||
function repl_connect (){
|
||||
echo "Connecting to $AMPY_PORT"
|
||||
screen $AMPY_PORT $BAUD_RATE
|
||||
}
|
||||
|
||||
function upy (){
|
||||
_command=${1}
|
||||
shift
|
||||
# echo "command $_command params $@"
|
||||
ampy -p $AMPY_PORT -b $BAUD_RATE ${_command} $@
|
||||
}
|
||||
Loading…
Reference in New Issue