diff --git a/640test.py b/640test.py new file mode 100644 index 0000000..e9228f0 --- /dev/null +++ b/640test.py @@ -0,0 +1,104 @@ +import RPi.GPIO as GPIO +import time +from Adafruit_PWM_Servo_Driver import PWM + +pwm = PWM( 0x60, debug=False) +pwm.setPWMFreq(1600) + +GPIO.setmode(GPIO.BCM) +GPIO.setup(17, GPIO.OUT) +# set mode to en/phase +GPIO.output(17, GPIO.HIGH) + +in1 = 5 +in2 = 4 + +in3 = 8 +in4 = 9 + +in5 = 0 +in6 = 1 + +in7 = 2 +in8 = 3 + +in9 = 10 +in10 = 11 + +in11 = 7 +in12 = 6 + +# Test motor one +# phase is 8, enable = 9 +pwm.setPWM(in1,0,0) +pwm.setPWM(in2,0,0) +pwm.setPWM(in3,0,0) +pwm.setPWM(in4,0,0) +pwm.setPWM(in5,0,0) +pwm.setPWM(in6,0,0) +pwm.setPWM(in7,0,0) +pwm.setPWM(in8,0,0) +pwm.setPWM(in9,0,0) +pwm.setPWM(in10,0,0) +pwm.setPWM(in11,0,0) +pwm.setPWM(in12,0,0) +time.sleep(2) +print "Set forward" +pwm.setPWM(in1,0,0) +pwm.setPWM(in2,0,4095) +pwm.setPWM(in3,0,0) +pwm.setPWM(in4,0,4095) +pwm.setPWM(in5,0,0) +pwm.setPWM(in6,0,4095) +pwm.setPWM(in7,0,0) +pwm.setPWM(in8,0,4095) +pwm.setPWM(in9,0,0) +pwm.setPWM(in10,0,4095) +pwm.setPWM(in11,0,0) +pwm.setPWM(in12,0,4095) +time.sleep(2) +print "stop" +pwm.setPWM(in1,0,0) +pwm.setPWM(in2,0,0) +pwm.setPWM(in3,0,0) +pwm.setPWM(in4,0,0) +pwm.setPWM(in5,0,0) +pwm.setPWM(in6,0,0) +pwm.setPWM(in7,0,0) +pwm.setPWM(in8,0,0) +pwm.setPWM(in9,0,0) +pwm.setPWM(in10,0,0) +pwm.setPWM(in11,0,0) +pwm.setPWM(in12,0,0) +time.sleep(2) +print "Set reverse" +pwm.setPWM(in1,0,4095) +pwm.setPWM(in2,0,4095) +pwm.setPWM(in3,0,4095) +pwm.setPWM(in4,0,4095) +pwm.setPWM(in5,0,4095) +pwm.setPWM(in6,0,4095) +pwm.setPWM(in7,0,4095) +pwm.setPWM(in8,0,4095) +pwm.setPWM(in9,0,4095) +pwm.setPWM(in10,0,4095) +pwm.setPWM(in11,0,4095) +pwm.setPWM(in12,0,4095) +time.sleep(2) +print "stop" +pwm.setPWM(in1,0,0) +pwm.setPWM(in2,0,0) +pwm.setPWM(in3,0,0) +pwm.setPWM(in4,0,0) +pwm.setPWM(in5,0,0) +pwm.setPWM(in6,0,0) +pwm.setPWM(in7,0,0) +pwm.setPWM(in8,0,0) +pwm.setPWM(in9,0,0) +pwm.setPWM(in10,0,0) +pwm.setPWM(in11,0,0) +pwm.setPWM(in12,0,0) + + +# cleanup +GPIO.cleanup() diff --git a/Adafruit_I2C.py b/Adafruit_I2C.py new file mode 100644 index 0000000..2b24b67 --- /dev/null +++ b/Adafruit_I2C.py @@ -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" diff --git a/Adafruit_PWM_Servo_Driver.py b/Adafruit_PWM_Servo_Driver.py new file mode 100644 index 0000000..35c993c --- /dev/null +++ b/Adafruit_PWM_Servo_Driver.py @@ -0,0 +1,92 @@ +#!/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): + "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 + 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 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) diff --git a/art.py b/art.py new file mode 100644 index 0000000..a0cdbd0 --- /dev/null +++ b/art.py @@ -0,0 +1,11 @@ +#!/usr/bin/env python +print(" ███▄ ▄███▓ ██▓ ▄████▄ ██▀███ ▒█████ ██▀███ ▒█████ ██▒ █▓") +print("▓██▒▀█▀ ██▒▓██▒▒██▀ ▀█ ▓██ ▒ ██▒▒██▒ ██▒▓██ ▒ ██▒▒██▒ ██▒▓██░ █▒") +print("▓██ ▓██░▒██▒▒▓█ ▄ ▓██ ░▄█ ▒▒██░ ██▒▓██ ░▄█ ▒▒██░ ██▒ ▓██ █▒░") +print("▒██ ▒██ ░██░▒▓▓▄ ▄██▒▒██▀▀█▄ ▒██ ██░▒██▀▀█▄ ▒██ ██░ ▒██ █░░") +print("▒██▒ ░██▒░██░▒ ▓███▀ ░░██▓ ▒██▒░ ████▓▒░░██▓ ▒██▒░ ████▓▒░ ▒▀█░ ") +print("░ ▒░ ░ ░░▓ ░ ░▒ ▒ ░░ ▒▓ ░▒▓░░ ▒░▒░▒░ ░ ▒▓ ░▒▓░░ ▒░▒░▒░ ░ ▐░ ") +print("░ ░ ░ ▒ ░ ░ ▒ ░▒ ░ ▒░ ░ ▒ ▒░ ░▒ ░ ▒░ ░ ▒ ▒░ ░ ░░ ") +print("░ ░ ▒ ░░ ░░ ░ ░ ░ ░ ▒ ░░ ░ ░ ░ ░ ▒ ░░ ") +print(" ░ ░ ░ ░ ░ ░ ░ ░ ░ ░ ░ ") +print(" ░ ░ ") diff --git a/dw640HAT.py b/dw640HAT.py new file mode 100644 index 0000000..f41afde --- /dev/null +++ b/dw640HAT.py @@ -0,0 +1,278 @@ +#!/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 = 0 #phase + in1 = 1 #enable + elif (num == 1): + in2 = 2 #phase + in1 = 3 #enable + elif (num == 2): + in2 = 7 #phase + in1 = 6 #enable + elif (num == 3): + in2 = 5 #phase + in1 = 4 #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) diff --git a/microrov.py b/microrov.py new file mode 100644 index 0000000..c040caa --- /dev/null +++ b/microrov.py @@ -0,0 +1,274 @@ +# -*- coding: utf-8 -*- +#!/usr/bin/env python +######################################################################## +# This example controls the GoPiGo and using a PS3 Dualshock 3 controller +# +# http://www.dexterindustries.com/GoPiGo/ +# History +# ------------------------------------------------ +# Author Date Comments +# Karan Nayan 11 July 14 Initial Authoring +''' +## License + GoPiGo for the Raspberry Pi: an open source robotics platform for the Raspberry Pi. + Copyright (C) 2015 Dexter Industries + +This program is free software: you can redistribute it and/or modify +it under the terms of the GNU General Public License as published by +the Free Software Foundation, either version 3 of the License, or +(at your option) any later version. + +This program is distributed in the hope that it will be useful, +but WITHOUT ANY WARRANTY; without even the implied warranty of +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +GNU General Public License for more details. + +You should have received a copy of the GNU General Public License +along with this program. If not, see . +''' +import serial, sys, time ,os, math +from ps3 import * #Import the PS3 library +import scrollphat +from dw640HAT import dw_MotorCONTROL, dw_DCMotor + +def showmotor(controller, row, motorval): + if controller == "ab": + row = row + 6 + + #print motorval + + if motorval == 0: + for n in range(5): + scrollphat.set_pixel(row,n,0) + scrollphat.update() + else: + steps = int(round((abs(motorval) - BASE_MOTOR_POWER) / 21)) + for n in range(5): + if n <= steps: + scrollphat.set_pixel(row,n,1) + scrollphat.update() + else: + scrollphat.set_pixel(row,n,0) + scrollphat.update() + + return + +def toggle(controller): + #sendcommand(controller.identifier + "-t();") + return + +def timerstart(controller): + #sendcommand(controller.identifier + "-ts();") + return + +def motor(controller, port, starboard, depth): + #mleft.setMotorSpeed(port) + #mright.setMotorSpeed(starboard) + #mvert.setMotorSpeed(depth) + #sendcommand(controller.identifier + "-m(" + str(port) + "," + str(starboard) + "," + str(depth) + ");") + return + +def port(controller, speed): + #print port + mleft.setMotorSpeed(speed) + + showmotor(controller, 2, speed) + + #print speed + #sendcommand(controller.identifier + "-p(" + port + ");") + return + +def starboard(controller, speed): + #print starboard + mright.setMotorSpeed(speed) + + showmotor(controller, 0, speed) + + #print speed + #sendcommand(controller.identifier + "-s(" + starboard + ");") + return + +def depth(controller, speed): + #print depth + mvert.setMotorSpeed(speed) + + showmotor(controller, 4, speed) + + #print speed + #sendcommand(controller.identifier + "-v(" + str(depth) + ");") + return + +def direction(controller, port, starboard): + #mleft.setMotorSpeed(port) + #mright.setMotorSpeed(starboard) + #sendcommand(controller.identifier + "-d(" + str(port) + "," + str(starboard) + ");") + return + +def sendcommand( cmd ): + #print(cmd.encode()) + return + +def tolarge( reading ): + return int(reading * 100) + +def calcleftmotor( x, y ): + #print(x) + #print(y) + v = (100 - abs(x)) * (y/100) + y + #print(v) + w = (100 - abs(y)) * (x/100) + x + #print(w) + speed = (v-w)/2 + #print(speed) + return int(speed) + +def calcrightmotor( x, y ): + v = (100 - abs(x)) * (y/100) + y + w = (100 - abs(y)) * (x/100) + x + speed = (v+w)/2 + #print(speed) + return int(speed) + +def translate(value,leftmin, leftmax, rightmin, rightmax): + leftspan = leftmax - leftmin + rightspan = rightmax - rightmin + + scaled = float(value - leftmin) / float(leftspan) + return int(rightmin + (scaled * rightspan)) + +# seconds we wait before sending another button press +BUTTON_LAG = 0.5 + +BASE_MOTOR_POWER = 150 + +scrollphat.clear() +scrollphat.set_brightness(2) + +# The start logo - all bloody for haloween +print(" ███▄ ▄███▓ ██▓ ▄████▄ ██▀███ ▒█████ ██▀███ ▒█████ ██▒ █▓") +print("▓██▒▀█▀ ██▒▓██▒▒██▀ ▀█ ▓██ ▒ ██▒▒██▒ ██▒▓██ ▒ ██▒▒██▒ ██▒▓██░ █▒") +print("▓██ ▓██░▒██▒▒▓█ ▄ ▓██ ░▄█ ▒▒██░ ██▒▓██ ░▄█ ▒▒██░ ██▒ ▓██ █▒░") +print("▒██ ▒██ ░██░▒▓▓▄ ▄██▒▒██▀▀█▄ ▒██ ██░▒██▀▀█▄ ▒██ ██░ ▒██ █░░") +print("▒██▒ ░██▒░██░▒ ▓███▀ ░░██▓ ▒██▒░ ████▓▒░░██▓ ▒██▒░ ████▓▒░ ▒▀█░ ") +print("░ ▒░ ░ ░░▓ ░ ░▒ ▒ ░░ ▒▓ ░▒▓░░ ▒░▒░▒░ ░ ▒▓ ░▒▓░░ ▒░▒░▒░ ░ ▐░ ") +print("░ ░ ░ ▒ ░ ░ ▒ ░▒ ░ ▒░ ░ ▒ ▒░ ░▒ ░ ▒░ ░ ▒ ▒░ ░ ░░ ") +print("░ ░ ▒ ░░ ░░ ░ ░ ░ ░ ▒ ░░ ░ ░ ░ ░ ▒ ░░ ") +print(" ░ ░ ░ ░ ░ ░ ░ ░ ░ ░ ░ ") +print(" ░ ░ ") + + +print("Initializing...") + +print("Attaching Motors") +dw = dw_MotorCONTROL( addr=0x60 ) +# Swap for different address if also using scrollphat +#dw = dw_MotorCONTROL( addr=0x61 ) +mleft = dw.getMotor(2) +mright = dw.getMotor(1) +mvert = dw.getMotor(3) + +controllerlist = [] + +# create a ps3 object - we want to handle up to two controllers +p=ps3() #Create a PS3 object +if p.joystick_count == 1: + print("Found controller 1 - ") + p.select_joystick(0) + controllerlist.append(p) + print("Setting ID to " + p.identifier) +elif p.joystick_count == 2: + print("Found controller 1 - ") + p.select_joystick(0) + controllerlist.append(p) + print("Setting ID to " + p.identifier) + ptwo=ps3() + print("Found controller 2 - ") + ptwo.select_joystick(1) + controllerlist.append(ptwo) + print("Setting ID to " + ptwo.identifier) + +print("Ready") +s=150 #Initialize +run=1 +flag=0 +tolerance=0.01 +while True: + try: + for ps in controllerlist: + ps.update() #Read the ps3 values + if ps.select: + # Select toggle button pressed + if (time.time() - ps.lastbuttontime) > BUTTON_LAG or ps.lastbuttontime == 0: + ps.lastbuttontime = time.time() + toggle(ps) + elif ps.start: + # Start / stop timer button pressed + if (time.time() - ps.lastbuttontime) > BUTTON_LAG or ps.lastbuttontime == 0: + ps.lastbuttontime = time.time() + timerstart(ps) + + if ps.r1: + if (time.time() - ps.lastbuttontime) > BUTTON_LAG or ps.lastbuttontime == 0: + ps.lastbuttontime = time.time() + motordepth = -255 + if motordepth != ps.lastvert: + ps.lastvert = motordepth + depth(ps, motordepth) + elif ps.r2: + if (time.time() - ps.lastbuttontime) > BUTTON_LAG or ps.lastbuttontime == 0: + ps.lastbuttontime = time.time() + motordepth = 255 + if motordepth != ps.lastvert: + ps.lastvert = motordepth + depth(ps, motordepth) + else: + motordepth = 0 + if motordepth != ps.lastvert: + ps.lastvert = motordepth + depth(ps, motordepth) + + + lefty=(ps.a_joystick_left_y) + righty=(ps.a_joystick_right_y) + + # control the left motor + if abs(lefty) > tolerance: + motorleft = -translate(tolarge(lefty), -100, 100, -255, 255) + if motorleft > 0 and motorleft < BASE_MOTOR_POWER: + motorleft = BASE_MOTOR_POWER + elif motorleft <0 and abs(motorleft) < BASE_MOTOR_POWER: + motorleft = -BASE_MOTOR_POWER + + if motorleft != ps.lastleft: + port(ps, motorleft) + ps.lastleft = motorleft + else: # at rest + motorleft = 0 + if motorleft != ps.lastleft: + port(ps, motorleft) + ps.lastleft = motorleft + + + # control the right motor + if abs(righty) > tolerance: + motorright = -translate(tolarge(righty), -100, 100, -255, 255) + if motorright > 0 and motorright < BASE_MOTOR_POWER: + motorright = BASE_MOTOR_POWER + elif motorright <0 and abs(motorright) < BASE_MOTOR_POWER: + motorright = -BASE_MOTOR_POWER + + if motorright != ps.lastright: + starboard(ps,motorright) + ps.lastright = motorright + else: + # at rest + motorright = 0 + if motorright != ps.lastright: + starboard(ps,motorright) + ps.lastright = motorright + + time.sleep(.01) + except KeyboardInterrupt: + scrollphat.clear() + dw.allOff() + sys.exit(-1) diff --git a/ps3.py b/ps3.py new file mode 100644 index 0000000..1e0dd67 --- /dev/null +++ b/ps3.py @@ -0,0 +1,133 @@ +#!/usr/bin/env python +######################################################################## +# This is the library to read values from PS3 Dualshock 3 controller +# +# http://www.dexterindustries.com/GoPiGo/ +# History +# ------------------------------------------------ +# Author Date Comments +# Karan Nayan 11 July 14 Initial Authoring +''' +## License + GoPiGo for the Raspberry Pi: an open source robotics platform for the Raspberry Pi. + Copyright (C) 2015 Dexter Industries + +This program is free software: you can redistribute it and/or modify +it under the terms of the GNU General Public License as published by +the Free Software Foundation, either version 3 of the License, or +(at your option) any later version. + +This program is distributed in the hope that it will be useful, +but WITHOUT ANY WARRANTY; without even the implied warranty of +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +GNU General Public License for more details. + +You should have received a copy of the GNU General Public License +along with this program. If not, see . +''' +# +# Dependencies- pygame +# Pairing the controller using bluetooth +# http://booting-rpi.blogspot.ro/2012/08/dualshock-3-and-raspberry-pi.html +# PS3 Key configuration http://wiki.ros.org/ps3joy +# +# Key values can be obtained by creating a ps3 object and calling update() regularly +######################################################################## +import pygame, sys, time ,os +from pygame.locals import * + +# initialise pygame outside of the object so only done once +pygame.init() +screen = pygame.display.set_mode((1, 1), pygame.NOFRAME) +pygame.joystick.init() +#PS3 functions and variables +class ps3: + #Initialize the controller when the oject is created + def __init__(self): + #Make the stdout buffer as 0,because of bug in Pygame which keeps on printing debug statements + #http://stackoverflow.com/questions/107705/python-output-buffering + #sys.stdout = os.fdopen(sys.stdout.fileno(), 'w', 0) + self.joystick_count = pygame.joystick.get_count() + #set to -1 to enfoce a 0 setting on first pass + self.lastleft = -1 + self.lastright = -1 + self.lastvert = -1 + self.lastbuttontime = 0 + # get count of joysticks=1, axes=27, buttons=19 for DualShock 3 + def select_joystick(self, num): + if num == 0: + self.identifier = "aa" + elif num == 1: + self.identifier = "ab" + self.joysticknum = num + self.joystick = pygame.joystick.Joystick(num) + self.joystick.init() + self.numaxes = self.joystick.get_numaxes() + self.numbuttons = self.joystick.get_numbuttons() + #Update the button values + def update(self): + loopQuit = False + button_state=[0]*self.numbuttons + button_analog=[0]*self.numaxes + #while loopQuit == False: + outstr = "" + + #Start suppressing the output on stdout from Pygame + #devnull = open('/dev/null', 'w') + #oldstdout_fno = os.dup(sys.stdout.fileno()) + #os.dup2(devnull.fileno(), 1) + + #Read analog values + for i in range(0,self.numaxes): + button_analog[i] = self.joystick.get_axis(i) + + #a_left =button_analog[] + self.a_right =button_analog[9] + self.a_up =button_analog[8] + self.a_down =button_analog[10] + self.a_l1 =button_analog[14] + self.a_l2 =button_analog[12] + self.a_r1 =button_analog[15] + self.a_r2 =button_analog[13] + self.a_triangle =button_analog[16] + self.a_circle =button_analog[17] + self.a_square =button_analog[19] + self.a_cross =button_analog[18] + + self.a_joystick_left_x =button_analog[0] + self.a_joystick_left_y =button_analog[1] + self.a_joystick_right_x =button_analog[2] + self.a_joystick_right_y =button_analog[3] + self.acc_x =button_analog[23] + self.acc_y =button_analog[24] + self.acc_z =button_analog[25] + + #Read digital values + for i in range(0,self.numbuttons): + button_state[i]=self.joystick.get_button(i) + self.select =button_state[0] + self.joystick_left =button_state[1] + self.joystick_right =button_state[2] + self.start =button_state[3] + self.up =button_state[4] + self.right =button_state[5] + self.down =button_state[6] + self.left =button_state[7] + self.l2 =button_state[8] + self.r2 =button_state[9] + self.l1 =button_state[10] + self.r1 =button_state[11] + self.triangle =button_state[12] + self.circle =button_state[13] + self.cross =button_state[14] + self.square =button_state[15] + self.ps =button_state[16] + + #Enable output on stdout + #os.dup2(oldstdout_fno, 1) + #os.close(oldstdout_fno) + + #refresh + pygame.event.get() + return button_analog + diff --git a/test.py b/test.py new file mode 100644 index 0000000..b24060e --- /dev/null +++ b/test.py @@ -0,0 +1,70 @@ +import pygame, sys, time +from pygame.locals import * + +pygame.init() +pygame.joystick.init() +joystick = pygame.joystick.Joystick(0) +joystick.init() +screen = pygame.display.set_mode((400,300)) +pygame.display.set_caption('Hello World') + +interval = 0.01 + +# get count of joysticks=1, axes=27, buttons=19 for DualShock 3 + +joystick_count = pygame.joystick.get_count() +print("joystick_count") +print(joystick_count) +print("--------------") + +numaxes = joystick.get_numaxes() +print("numaxes") +print(numaxes) +print("--------------") + +numbuttons = joystick.get_numbuttons() +print("numbuttons") +print(numbuttons) +print("--------------") + +loopQuit = False +while loopQuit == False: + + # test joystick axes + # outstr = "" + # for i in range(0,4): + # axis = joystick.get_axis(i) + # outstr = outstr + str(i) + ":" + str(axis) + "|" + # print(outstr) + + # test controller buttons + #outstr = "" + #for i in range(0,numbuttons): + # button = joystick.get_button(i) + # outstr = outstr + str(i) + ":" + str(button) + "|" + #print(outstr) + + for event in pygame.event.get(): + if event.type == QUIT: + loopQuit = True + elif event.type == pygame.KEYDOWN: + if event.key == pygame.K_ESCAPE: + loopQuit = True + + # other event tests, but polling seems to work better in main loop + if event.type == pygame.JOYBUTTONDOWN: + print("joy button down") + if event.type == pygame.JOYBUTTONUP: + print("joy button up") + if event.type == pygame.JOYBALLMOTION: + print("joy ball motion") + # axis motion is movement of controller + # dominates events when used + #if event.type == pygame.JOYAXISMOTION: + # print("joy axis motion") + + pygame.display.update() + time.sleep(interval) + +pygame.quit() +sys.exit()