Change structure
This commit is contained in:
shrkey
161
Python/dw640HAT/Adafruit_I2C.py
Normal file
161
Python/dw640HAT/Adafruit_I2C.py
Normal file
@@ -0,0 +1,161 @@
|
||||
#!/usr/bin/python
|
||||
import re
|
||||
import smbus
|
||||
|
||||
# ===========================================================================
|
||||
# Adafruit_I2C Class
|
||||
# ===========================================================================
|
||||
|
||||
class Adafruit_I2C(object):
|
||||
|
||||
@staticmethod
|
||||
def getPiRevision():
|
||||
"Gets the version number of the Raspberry Pi board"
|
||||
# Revision list available at: http://elinux.org/RPi_HardwareHistory#Board_Revision_History
|
||||
try:
|
||||
with open('/proc/cpuinfo', 'r') as infile:
|
||||
for line in infile:
|
||||
# Match a line of the form "Revision : 0002" while ignoring extra
|
||||
# info in front of the revsion (like 1000 when the Pi was over-volted).
|
||||
match = re.match('Revision\s+:\s+.*(\w{4})$', line)
|
||||
if match and match.group(1) in ['0000', '0002', '0003']:
|
||||
# Return revision 1 if revision ends with 0000, 0002 or 0003.
|
||||
return 1
|
||||
elif match:
|
||||
# Assume revision 2 if revision ends with any other 4 chars.
|
||||
return 2
|
||||
# Couldn't find the revision, assume revision 0 like older code for compatibility.
|
||||
return 0
|
||||
except:
|
||||
return 0
|
||||
|
||||
@staticmethod
|
||||
def getPiI2CBusNumber():
|
||||
# Gets the I2C bus number /dev/i2c#
|
||||
return 1 if Adafruit_I2C.getPiRevision() > 1 else 0
|
||||
|
||||
def __init__(self, address, busnum=-1, debug=False):
|
||||
self.address = address
|
||||
# By default, the correct I2C bus is auto-detected using /proc/cpuinfo
|
||||
# Alternatively, you can hard-code the bus version below:
|
||||
# self.bus = smbus.SMBus(0); # Force I2C0 (early 256MB Pi's)
|
||||
# self.bus = smbus.SMBus(1); # Force I2C1 (512MB Pi's)
|
||||
self.bus = smbus.SMBus(busnum if busnum >= 0 else Adafruit_I2C.getPiI2CBusNumber())
|
||||
self.debug = debug
|
||||
|
||||
def reverseByteOrder(self, data):
|
||||
"Reverses the byte order of an int (16-bit) or long (32-bit) value"
|
||||
# Courtesy Vishal Sapre
|
||||
byteCount = len(hex(data)[2:].replace('L','')[::2])
|
||||
val = 0
|
||||
for i in range(byteCount):
|
||||
val = (val << 8) | (data & 0xff)
|
||||
data >>= 8
|
||||
return val
|
||||
|
||||
def errMsg(self):
|
||||
print "Error accessing 0x%02X: Check your I2C address" % self.address
|
||||
return -1
|
||||
|
||||
def write8(self, reg, value):
|
||||
"Writes an 8-bit value to the specified register/address"
|
||||
try:
|
||||
self.bus.write_byte_data(self.address, reg, value)
|
||||
if self.debug:
|
||||
print "I2C: Wrote 0x%02X to register 0x%02X" % (value, reg)
|
||||
except IOError, err:
|
||||
return self.errMsg()
|
||||
|
||||
def write16(self, reg, value):
|
||||
"Writes a 16-bit value to the specified register/address pair"
|
||||
try:
|
||||
self.bus.write_word_data(self.address, reg, value)
|
||||
if self.debug:
|
||||
print ("I2C: Wrote 0x%02X to register pair 0x%02X,0x%02X" %
|
||||
(value, reg, reg+1))
|
||||
except IOError, err:
|
||||
return self.errMsg()
|
||||
|
||||
def writeRaw8(self, value):
|
||||
"Writes an 8-bit value on the bus"
|
||||
try:
|
||||
self.bus.write_byte(self.address, value)
|
||||
if self.debug:
|
||||
print "I2C: Wrote 0x%02X" % value
|
||||
except IOError, err:
|
||||
return self.errMsg()
|
||||
|
||||
def writeList(self, reg, list):
|
||||
"Writes an array of bytes using I2C format"
|
||||
try:
|
||||
if self.debug:
|
||||
print "I2C: Writing list to register 0x%02X:" % reg
|
||||
print list
|
||||
self.bus.write_i2c_block_data(self.address, reg, list)
|
||||
except IOError, err:
|
||||
return self.errMsg()
|
||||
|
||||
def readList(self, reg, length):
|
||||
"Read a list of bytes from the I2C device"
|
||||
try:
|
||||
results = self.bus.read_i2c_block_data(self.address, reg, length)
|
||||
if self.debug:
|
||||
print ("I2C: Device 0x%02X returned the following from reg 0x%02X" %
|
||||
(self.address, reg))
|
||||
print results
|
||||
return results
|
||||
except IOError, err:
|
||||
return self.errMsg()
|
||||
|
||||
def readU8(self, reg):
|
||||
"Read an unsigned byte from the I2C device"
|
||||
try:
|
||||
result = self.bus.read_byte_data(self.address, reg)
|
||||
if self.debug:
|
||||
print ("I2C: Device 0x%02X returned 0x%02X from reg 0x%02X" %
|
||||
(self.address, result & 0xFF, reg))
|
||||
return result
|
||||
except IOError, err:
|
||||
return self.errMsg()
|
||||
|
||||
def readS8(self, reg):
|
||||
"Reads a signed byte from the I2C device"
|
||||
try:
|
||||
result = self.bus.read_byte_data(self.address, reg)
|
||||
if result > 127: result -= 256
|
||||
if self.debug:
|
||||
print ("I2C: Device 0x%02X returned 0x%02X from reg 0x%02X" %
|
||||
(self.address, result & 0xFF, reg))
|
||||
return result
|
||||
except IOError, err:
|
||||
return self.errMsg()
|
||||
|
||||
def readU16(self, reg, little_endian=True):
|
||||
"Reads an unsigned 16-bit value from the I2C device"
|
||||
try:
|
||||
result = self.bus.read_word_data(self.address,reg)
|
||||
# Swap bytes if using big endian because read_word_data assumes little
|
||||
# endian on ARM (little endian) systems.
|
||||
if not little_endian:
|
||||
result = ((result << 8) & 0xFF00) + (result >> 8)
|
||||
if (self.debug):
|
||||
print "I2C: Device 0x%02X returned 0x%04X from reg 0x%02X" % (self.address, result & 0xFFFF, reg)
|
||||
return result
|
||||
except IOError, err:
|
||||
return self.errMsg()
|
||||
|
||||
def readS16(self, reg, little_endian=True):
|
||||
"Reads a signed 16-bit value from the I2C device"
|
||||
try:
|
||||
result = self.readU16(reg,little_endian)
|
||||
if result > 32767: result -= 65536
|
||||
return result
|
||||
except IOError, err:
|
||||
return self.errMsg()
|
||||
|
||||
if __name__ == '__main__':
|
||||
try:
|
||||
bus = Adafruit_I2C(address=0)
|
||||
print "Default I2C bus is accessible"
|
||||
except:
|
||||
print "Error accessing default I2C bus"
|
||||
143
Python/dw640HAT/Adafruit_PWM_Servo_Driver.py
Normal file
143
Python/dw640HAT/Adafruit_PWM_Servo_Driver.py
Normal file
@@ -0,0 +1,143 @@
|
||||
#!/usr/bin/python
|
||||
|
||||
import time
|
||||
import math
|
||||
from Adafruit_I2C import Adafruit_I2C
|
||||
|
||||
# ============================================================================
|
||||
# Adafruit PCA9685 16-Channel PWM Servo Driver
|
||||
# ============================================================================
|
||||
|
||||
class PWM :
|
||||
# Registers/etc.
|
||||
__MODE1 = 0x00
|
||||
__MODE2 = 0x01
|
||||
__SUBADR1 = 0x02
|
||||
__SUBADR2 = 0x03
|
||||
__SUBADR3 = 0x04
|
||||
__PRESCALE = 0xFE
|
||||
__LED0_ON_L = 0x06
|
||||
__LED0_ON_H = 0x07
|
||||
__LED0_OFF_L = 0x08
|
||||
__LED0_OFF_H = 0x09
|
||||
__ALL_LED_ON_L = 0xFA
|
||||
__ALL_LED_ON_H = 0xFB
|
||||
__ALL_LED_OFF_L = 0xFC
|
||||
__ALL_LED_OFF_H = 0xFD
|
||||
|
||||
# Bits
|
||||
__RESTART = 0x80
|
||||
__SLEEP = 0x10
|
||||
__ALLCALL = 0x01
|
||||
__INVRT = 0x10
|
||||
__OUTDRV = 0x04
|
||||
|
||||
general_call_i2c = Adafruit_I2C(0x00)
|
||||
|
||||
@classmethod
|
||||
def softwareReset(cls):
|
||||
"Sends a software reset (SWRST) command to all the servo drivers on the bus"
|
||||
cls.general_call_i2c.writeRaw8(0x06) # SWRST
|
||||
|
||||
def __init__(self, address=0x40, debug=False):
|
||||
self.i2c = Adafruit_I2C(address)
|
||||
self.i2c.debug = debug
|
||||
self.address = address
|
||||
self.debug = debug
|
||||
if (self.debug):
|
||||
print "Reseting PCA9685 MODE1 (without SLEEP) and MODE2"
|
||||
self.setAllPWM(0, 0)
|
||||
self.i2c.write8(self.__MODE2, self.__OUTDRV)
|
||||
self.i2c.write8(self.__MODE1, self.__ALLCALL)
|
||||
time.sleep(0.005) # wait for oscillator
|
||||
|
||||
mode1 = self.i2c.readU8(self.__MODE1)
|
||||
mode1 = mode1 & ~self.__SLEEP # wake up (reset sleep)
|
||||
self.i2c.write8(self.__MODE1, mode1)
|
||||
time.sleep(0.005) # wait for oscillator
|
||||
|
||||
def setPWMFreq(self, freq, correctionFactor=1.0):
|
||||
"Sets the PWM frequency"
|
||||
prescaleval = 25000000.0 # 25MHz
|
||||
prescaleval /= 4096.0 # 12-bit
|
||||
prescaleval /= float(freq)
|
||||
prescaleval -= 1.0
|
||||
if (self.debug):
|
||||
print "Setting PWM frequency to %d Hz" % freq
|
||||
print "Estimated pre-scale: %d" % prescaleval
|
||||
prescale = round(prescaleval * correctionFactor + 0.5)
|
||||
if (self.debug):
|
||||
print "Final pre-scale: %d" % prescale
|
||||
|
||||
oldmode = self.i2c.readU8(self.__MODE1);
|
||||
newmode = (oldmode & 0x7F) | 0x10 # sleep
|
||||
self.i2c.write8(self.__MODE1, newmode) # go to sleep
|
||||
self.i2c.write8(self.__PRESCALE, int(math.floor(prescale)))
|
||||
self.i2c.write8(self.__MODE1, oldmode)
|
||||
time.sleep(0.005)
|
||||
self.i2c.write8(self.__MODE1, oldmode | 0x80)
|
||||
|
||||
def setPWMFreqMin(self, freq, correctionFactor=1.0):
|
||||
"Sets the PWM frequency"
|
||||
prescaleval = 25000000.0 # 25MHz
|
||||
prescaleval /= 4096.0 # 12-bit
|
||||
prescaleval /= float(freq)
|
||||
prescaleval -= 1.0
|
||||
if (self.debug):
|
||||
print "Setting PWM frequency to %d Hz" % freq
|
||||
print "Estimated pre-scale: %d" % prescaleval
|
||||
prescale = math.floor(prescaleval * correctionFactor + 0.5)
|
||||
if (self.debug):
|
||||
print "Final pre-scale: %d" % prescale
|
||||
|
||||
oldmode = self.i2c.readU8(self.__MODE1);
|
||||
newmode = (oldmode & 0x7F) | 0x10 # sleep
|
||||
self.i2c.write8(self.__MODE1, newmode) # go to sleep
|
||||
self.i2c.write8(self.__PRESCALE, int(math.floor(prescale)))
|
||||
self.i2c.write8(self.__MODE1, oldmode)
|
||||
time.sleep(0.005)
|
||||
self.i2c.write8(self.__MODE1, oldmode | 0x80)
|
||||
|
||||
def setPWMFreqMax(self, freq, correctionFactor=1.0):
|
||||
"Sets the PWM frequency"
|
||||
prescaleval = 25000000.0 # 25MHz
|
||||
prescaleval /= 4096.0 # 12-bit
|
||||
prescaleval /= float(freq)
|
||||
prescaleval -= 1.0
|
||||
if (self.debug):
|
||||
print "Setting PWM frequency to %d Hz" % freq
|
||||
print "Estimated pre-scale: %d" % prescaleval
|
||||
prescale = math.ceil(prescaleval * correctionFactor + 0.5)
|
||||
if (self.debug):
|
||||
print "Final pre-scale: %d" % prescale
|
||||
|
||||
oldmode = self.i2c.readU8(self.__MODE1);
|
||||
newmode = (oldmode & 0x7F) | 0x10 # sleep
|
||||
self.i2c.write8(self.__MODE1, newmode) # go to sleep
|
||||
self.i2c.write8(self.__PRESCALE, int(math.floor(prescale)))
|
||||
self.i2c.write8(self.__MODE1, oldmode)
|
||||
time.sleep(0.005)
|
||||
self.i2c.write8(self.__MODE1, oldmode | 0x80)
|
||||
|
||||
def getPWMFreq(self):
|
||||
prescale = self.i2c.readU8(self.__PRESCALE)
|
||||
calcfreq = 25000000.0 / 4096.0 / ( float(prescale) + 1 )
|
||||
if (self.debug):
|
||||
print "Got pre-scale: %d" % prescale
|
||||
print "Calculated Frequency: %d" % calcfreq
|
||||
|
||||
return calcfreq
|
||||
|
||||
def setPWM(self, channel, on, off):
|
||||
"Sets a single PWM channel"
|
||||
self.i2c.write8(self.__LED0_ON_L+4*channel, on & 0xFF)
|
||||
self.i2c.write8(self.__LED0_ON_H+4*channel, on >> 8)
|
||||
self.i2c.write8(self.__LED0_OFF_L+4*channel, off & 0xFF)
|
||||
self.i2c.write8(self.__LED0_OFF_H+4*channel, off >> 8)
|
||||
|
||||
def setAllPWM(self, on, off):
|
||||
"Sets a all PWM channels"
|
||||
self.i2c.write8(self.__ALL_LED_ON_L, on & 0xFF)
|
||||
self.i2c.write8(self.__ALL_LED_ON_H, on >> 8)
|
||||
self.i2c.write8(self.__ALL_LED_OFF_L, off & 0xFF)
|
||||
self.i2c.write8(self.__ALL_LED_OFF_H, off >> 8)
|
||||
1
Python/dw640HAT/__init__.py
Normal file
1
Python/dw640HAT/__init__.py
Normal file
@@ -0,0 +1 @@
|
||||
from .dw640HAT import dw_DCMotor, dw_MotorCONTROL
|
||||
275
Python/dw640HAT/dw640HAT.py
Normal file
275
Python/dw640HAT/dw640HAT.py
Normal file
@@ -0,0 +1,275 @@
|
||||
#!/usr/bin/python
|
||||
|
||||
import RPi.GPIO as GPIO
|
||||
from Adafruit_PWM_Servo_Driver import PWM
|
||||
import time
|
||||
|
||||
# class Adafruit_StepperMotor:
|
||||
# MICROSTEPS = 8
|
||||
# MICROSTEP_CURVE = [0, 50, 98, 142, 180, 212, 236, 250, 255]
|
||||
#
|
||||
# #MICROSTEPS = 16
|
||||
# # a sinusoidal curve NOT LINEAR!
|
||||
# #MICROSTEP_CURVE = [0, 25, 50, 74, 98, 120, 141, 162, 180, 197, 212, 225, 236, 244, 250, 253, 255]
|
||||
#
|
||||
# def __init__(self, controller, num, steps=200):
|
||||
# self.MC = controller
|
||||
# self.revsteps = steps
|
||||
# self.motornum = num
|
||||
# self.sec_per_step = 0.1
|
||||
# self.steppingcounter = 0
|
||||
# self.currentstep = 0
|
||||
#
|
||||
# num -= 1
|
||||
#
|
||||
# if (num == 0):
|
||||
# self.PWMA = 8
|
||||
# self.AIN2 = 9
|
||||
# self.AIN1 = 10
|
||||
# self.PWMB = 13
|
||||
# self.BIN2 = 12
|
||||
# self.BIN1 = 11
|
||||
# elif (num == 1):
|
||||
# self.PWMA = 2
|
||||
# self.AIN2 = 3
|
||||
# self.AIN1 = 4
|
||||
# self.PWMB = 7
|
||||
# self.BIN2 = 6
|
||||
# self.BIN1 = 5
|
||||
# else:
|
||||
# raise NameError('MotorHAT Stepper must be between 1 and 2 inclusive')
|
||||
#
|
||||
# def setSpeed(self, rpm):
|
||||
# self.sec_per_step = 60.0 / (self.revsteps * rpm)
|
||||
# self.steppingcounter = 0
|
||||
#
|
||||
# def oneStep(self, dir, style):
|
||||
# pwm_a = pwm_b = 255
|
||||
#
|
||||
# # first determine what sort of stepping procedure we're up to
|
||||
# if (style == Adafruit_MotorHAT.SINGLE):
|
||||
# if ((self.currentstep/(self.MICROSTEPS/2)) % 2):
|
||||
# # we're at an odd step, weird
|
||||
# if (dir == Adafruit_MotorHAT.FORWARD):
|
||||
# self.currentstep += self.MICROSTEPS/2
|
||||
# else:
|
||||
# self.currentstep -= self.MICROSTEPS/2
|
||||
# else:
|
||||
# # go to next even step
|
||||
# if (dir == Adafruit_MotorHAT.FORWARD):
|
||||
# self.currentstep += self.MICROSTEPS
|
||||
# else:
|
||||
# self.currentstep -= self.MICROSTEPS
|
||||
# if (style == Adafruit_MotorHAT.DOUBLE):
|
||||
# if not (self.currentstep/(self.MICROSTEPS/2) % 2):
|
||||
# # we're at an even step, weird
|
||||
# if (dir == Adafruit_MotorHAT.FORWARD):
|
||||
# self.currentstep += self.MICROSTEPS/2
|
||||
# else:
|
||||
# self.currentstep -= self.MICROSTEPS/2
|
||||
# else:
|
||||
# # go to next odd step
|
||||
# if (dir == Adafruit_MotorHAT.FORWARD):
|
||||
# self.currentstep += self.MICROSTEPS
|
||||
# else:
|
||||
# self.currentstep -= self.MICROSTEPS
|
||||
# if (style == Adafruit_MotorHAT.INTERLEAVE):
|
||||
# if (dir == Adafruit_MotorHAT.FORWARD):
|
||||
# self.currentstep += self.MICROSTEPS/2
|
||||
# else:
|
||||
# self.currentstep -= self.MICROSTEPS/2
|
||||
#
|
||||
# if (style == Adafruit_MotorHAT.MICROSTEP):
|
||||
# if (dir == Adafruit_MotorHAT.FORWARD):
|
||||
# self.currentstep += 1
|
||||
# else:
|
||||
# self.currentstep -= 1
|
||||
#
|
||||
# # go to next 'step' and wrap around
|
||||
# self.currentstep += self.MICROSTEPS * 4
|
||||
# self.currentstep %= self.MICROSTEPS * 4
|
||||
#
|
||||
# pwm_a = pwm_b = 0
|
||||
# if (self.currentstep >= 0) and (self.currentstep < self.MICROSTEPS):
|
||||
# pwm_a = self.MICROSTEP_CURVE[self.MICROSTEPS - self.currentstep]
|
||||
# pwm_b = self.MICROSTEP_CURVE[self.currentstep]
|
||||
# elif (self.currentstep >= self.MICROSTEPS) and (self.currentstep < self.MICROSTEPS*2):
|
||||
# pwm_a = self.MICROSTEP_CURVE[self.currentstep - self.MICROSTEPS]
|
||||
# pwm_b = self.MICROSTEP_CURVE[self.MICROSTEPS*2 - self.currentstep]
|
||||
# elif (self.currentstep >= self.MICROSTEPS*2) and (self.currentstep < self.MICROSTEPS*3):
|
||||
# pwm_a = self.MICROSTEP_CURVE[self.MICROSTEPS*3 - self.currentstep]
|
||||
# pwm_b = self.MICROSTEP_CURVE[self.currentstep - self.MICROSTEPS*2]
|
||||
# elif (self.currentstep >= self.MICROSTEPS*3) and (self.currentstep < self.MICROSTEPS*4):
|
||||
# pwm_a = self.MICROSTEP_CURVE[self.currentstep - self.MICROSTEPS*3]
|
||||
# pwm_b = self.MICROSTEP_CURVE[self.MICROSTEPS*4 - self.currentstep]
|
||||
#
|
||||
#
|
||||
# # go to next 'step' and wrap around
|
||||
# self.currentstep += self.MICROSTEPS * 4
|
||||
# self.currentstep %= self.MICROSTEPS * 4
|
||||
#
|
||||
# # only really used for microstepping, otherwise always on!
|
||||
# self.MC._pwm.setPWM(self.PWMA, 0, pwm_a*16)
|
||||
# self.MC._pwm.setPWM(self.PWMB, 0, pwm_b*16)
|
||||
#
|
||||
# # set up coil energizing!
|
||||
# coils = [0, 0, 0, 0]
|
||||
#
|
||||
# if (style == Adafruit_MotorHAT.MICROSTEP):
|
||||
# if (self.currentstep >= 0) and (self.currentstep < self.MICROSTEPS):
|
||||
# coils = [1, 1, 0, 0]
|
||||
# elif (self.currentstep >= self.MICROSTEPS) and (self.currentstep < self.MICROSTEPS*2):
|
||||
# coils = [0, 1, 1, 0]
|
||||
# elif (self.currentstep >= self.MICROSTEPS*2) and (self.currentstep < self.MICROSTEPS*3):
|
||||
# coils = [0, 0, 1, 1]
|
||||
# elif (self.currentstep >= self.MICROSTEPS*3) and (self.currentstep < self.MICROSTEPS*4):
|
||||
# coils = [1, 0, 0, 1]
|
||||
# else:
|
||||
# step2coils = [ [1, 0, 0, 0],
|
||||
# [1, 1, 0, 0],
|
||||
# [0, 1, 0, 0],
|
||||
# [0, 1, 1, 0],
|
||||
# [0, 0, 1, 0],
|
||||
# [0, 0, 1, 1],
|
||||
# [0, 0, 0, 1],
|
||||
# [1, 0, 0, 1] ]
|
||||
# coils = step2coils[self.currentstep/(self.MICROSTEPS/2)]
|
||||
#
|
||||
# #print "coils state = " + str(coils)
|
||||
# self.MC.setPin(self.AIN2, coils[0])
|
||||
# self.MC.setPin(self.BIN1, coils[1])
|
||||
# self.MC.setPin(self.AIN1, coils[2])
|
||||
# self.MC.setPin(self.BIN2, coils[3])
|
||||
#
|
||||
# return self.currentstep
|
||||
#
|
||||
# def step(self, steps, direction, stepstyle):
|
||||
# s_per_s = self.sec_per_step
|
||||
# lateststep = 0
|
||||
#
|
||||
# if (stepstyle == Adafruit_MotorHAT.INTERLEAVE):
|
||||
# s_per_s = s_per_s / 2.0
|
||||
# if (stepstyle == Adafruit_MotorHAT.MICROSTEP):
|
||||
# s_per_s /= self.MICROSTEPS
|
||||
# steps *= self.MICROSTEPS
|
||||
#
|
||||
# print s_per_s, " sec per step"
|
||||
#
|
||||
# for s in range(steps):
|
||||
# lateststep = self.oneStep(direction, stepstyle)
|
||||
# time.sleep(s_per_s)
|
||||
#
|
||||
# if (stepstyle == Adafruit_MotorHAT.MICROSTEP):
|
||||
# # this is an edge case, if we are in between full steps, lets just keep going
|
||||
# # so we end on a full step
|
||||
# while (lateststep != 0) and (lateststep != self.MICROSTEPS):
|
||||
# lateststep = self.oneStep(dir, stepstyle)
|
||||
# time.sleep(s_per_s)
|
||||
|
||||
class dw_DCMotor:
|
||||
def __init__(self, controller, num):
|
||||
self.speed = 0
|
||||
self.MC = controller
|
||||
self.motornum = num
|
||||
modepin = in1 = in2 = 0
|
||||
|
||||
if (num == 0):
|
||||
in2 = 2 #phase
|
||||
in1 = 3 #enable
|
||||
elif (num == 1):
|
||||
in2 = 4 #phase
|
||||
in1 = 5 #enable
|
||||
elif (num == 2):
|
||||
in2 = 15 #phase
|
||||
in1 = 14 #enable
|
||||
elif (num == 3):
|
||||
in2 = 13 #phase
|
||||
in1 = 12 #enable
|
||||
elif (num == 4):
|
||||
in2 = 8 #phase
|
||||
in1 = 9 #enable
|
||||
elif (num == 5):
|
||||
in2 = 10 #phase
|
||||
in1 = 11 #enable
|
||||
else:
|
||||
raise NameError('MotorHAT Motor must be between 1 and 6 inclusive')
|
||||
|
||||
self.PHpin = in2
|
||||
self.ENpin = in1
|
||||
# switch off
|
||||
self.run(dw_MotorCONTROL.RELEASE, 0)
|
||||
|
||||
def setMotorSpeed(self, value):
|
||||
if(value > 0) and (value <= 255):
|
||||
self.run(dw_MotorCONTROL.FORWARD, value)
|
||||
if(value == 0):
|
||||
self.run(dw_MotorCONTROL.RELEASE, value)
|
||||
if(value < 0) and (value >= -255):
|
||||
self.run(dw_MotorCONTROL.BACKWARD, abs(value))
|
||||
|
||||
def run(self, command, speed = 0):
|
||||
if not self.MC:
|
||||
return
|
||||
if (command == dw_MotorCONTROL.FORWARD):
|
||||
self.MC.setPin(self.PHpin, 0)
|
||||
self.MC._pwm.setPWM(self.ENpin, 0, speed*16)
|
||||
if (command == dw_MotorCONTROL.BACKWARD):
|
||||
self.MC.setPin(self.PHpin, 1)
|
||||
self.MC._pwm.setPWM(self.ENpin, 0, speed*16)
|
||||
if (command == dw_MotorCONTROL.RELEASE):
|
||||
self.MC.setPin(self.PHpin, 0)
|
||||
self.MC.setPin(self.ENpin, 0)
|
||||
|
||||
def setSpeed(self, speed):
|
||||
if (speed < 0):
|
||||
speed = 0
|
||||
if (speed > 255):
|
||||
speed = 255
|
||||
self.MC._pwm.setPWM(self.ENpin, 0, speed*16)
|
||||
|
||||
class dw_MotorCONTROL:
|
||||
FORWARD = 1
|
||||
BACKWARD = 2
|
||||
BRAKE = 3
|
||||
RELEASE = 4
|
||||
|
||||
SINGLE = 1
|
||||
DOUBLE = 2
|
||||
INTERLEAVE = 3
|
||||
MICROSTEP = 4
|
||||
|
||||
ININ = 0
|
||||
PHASE = 1
|
||||
|
||||
def __init__(self, addr = 0x60, freq = 1600):
|
||||
self._i2caddr = addr # default addr on HAT
|
||||
self._frequency = freq # default @1600Hz PWM freq
|
||||
# self.steppers = [ Adafruit_StepperMotor(self, 1), Adafruit_StepperMotor(self, 2) ]
|
||||
self._pwm = PWM(addr, debug=False)
|
||||
self._pwm.setPWMFreq(self._frequency)
|
||||
# Just gonna default to high for now
|
||||
GPIO.setmode(GPIO.BCM)
|
||||
GPIO.setwarnings(False)
|
||||
GPIO.setup(17, GPIO.OUT)
|
||||
GPIO.output(17, GPIO.HIGH)
|
||||
|
||||
self.motors = [ dw_DCMotor(self, m) for m in range(6) ]
|
||||
|
||||
def setPin(self, pin, value):
|
||||
if (pin < 0) or (pin > 15):
|
||||
raise NameError('PWM pin must be between 0 and 15 inclusive')
|
||||
if (value != 0) and (value != 1):
|
||||
raise NameError('Pin value must be 0 or 1!')
|
||||
if (value == 0):
|
||||
self._pwm.setPWM(pin, 0, 4096)
|
||||
if (value == 1):
|
||||
self._pwm.setPWM(pin, 4096, 0)
|
||||
|
||||
def getMotor(self, num):
|
||||
if (num < 1) or (num > 6):
|
||||
raise NameError('MotorHAT Motor must be between 1 and 6 inclusive')
|
||||
return self.motors[num-1]
|
||||
|
||||
def allOff(self):
|
||||
for y in range(5):
|
||||
self.motors[y].run(dw_MotorCONTROL.RELEASE, 0)
|
||||
21
Python/examples/640test.py
Normal file
21
Python/examples/640test.py
Normal file
@@ -0,0 +1,21 @@
|
||||
import time
|
||||
from dw640HAT import dw_MotorCONTROL, dw_DCMotor
|
||||
|
||||
dw = dw_MotorCONTROL( addr=0x60 )
|
||||
m = dw.getMotor(1)
|
||||
|
||||
m.run(dw_MotorCONTROL.RELEASE)
|
||||
time.sleep(1)
|
||||
|
||||
##time.sleep(10)
|
||||
print "Set forward"
|
||||
m.setMotorSpeed(255)
|
||||
time.sleep(1)
|
||||
print "stop"
|
||||
m.setMotorSpeed(0)
|
||||
time.sleep(1)
|
||||
print "Set reverse"
|
||||
m.setMotorSpeed(-255)
|
||||
time.sleep(1)
|
||||
print "stop"
|
||||
m.run(dw_MotorCONTROL.RELEASE)
|
||||
Reference in New Issue
Block a user