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