Initial check in
This commit is contained in:
shrkey
104
640test.py
Normal file
104
640test.py
Normal file
@@ -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()
|
||||||
161
Adafruit_I2C.py
Normal file
161
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"
|
||||||
92
Adafruit_PWM_Servo_Driver.py
Normal file
92
Adafruit_PWM_Servo_Driver.py
Normal file
@@ -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)
|
||||||
11
art.py
Normal file
11
art.py
Normal file
@@ -0,0 +1,11 @@
|
|||||||
|
#!/usr/bin/env python
|
||||||
|
print(" ███▄ ▄███▓ ██▓ ▄████▄ ██▀███ ▒█████ ██▀███ ▒█████ ██▒ █▓")
|
||||||
|
print("▓██▒▀█▀ ██▒▓██▒▒██▀ ▀█ ▓██ ▒ ██▒▒██▒ ██▒▓██ ▒ ██▒▒██▒ ██▒▓██░ █▒")
|
||||||
|
print("▓██ ▓██░▒██▒▒▓█ ▄ ▓██ ░▄█ ▒▒██░ ██▒▓██ ░▄█ ▒▒██░ ██▒ ▓██ █▒░")
|
||||||
|
print("▒██ ▒██ ░██░▒▓▓▄ ▄██▒▒██▀▀█▄ ▒██ ██░▒██▀▀█▄ ▒██ ██░ ▒██ █░░")
|
||||||
|
print("▒██▒ ░██▒░██░▒ ▓███▀ ░░██▓ ▒██▒░ ████▓▒░░██▓ ▒██▒░ ████▓▒░ ▒▀█░ ")
|
||||||
|
print("░ ▒░ ░ ░░▓ ░ ░▒ ▒ ░░ ▒▓ ░▒▓░░ ▒░▒░▒░ ░ ▒▓ ░▒▓░░ ▒░▒░▒░ ░ ▐░ ")
|
||||||
|
print("░ ░ ░ ▒ ░ ░ ▒ ░▒ ░ ▒░ ░ ▒ ▒░ ░▒ ░ ▒░ ░ ▒ ▒░ ░ ░░ ")
|
||||||
|
print("░ ░ ▒ ░░ ░░ ░ ░ ░ ░ ▒ ░░ ░ ░ ░ ░ ▒ ░░ ")
|
||||||
|
print(" ░ ░ ░ ░ ░ ░ ░ ░ ░ ░ ░ ")
|
||||||
|
print(" ░ ░ ")
|
||||||
278
dw640HAT.py
Normal file
278
dw640HAT.py
Normal file
@@ -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)
|
||||||
274
microrov.py
Normal file
274
microrov.py
Normal file
@@ -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 <http://www.gnu.org/licenses/gpl-3.0.txt>.
|
||||||
|
'''
|
||||||
|
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)
|
||||||
133
ps3.py
Normal file
133
ps3.py
Normal file
@@ -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 <http://www.gnu.org/licenses/gpl-3.0.txt>.
|
||||||
|
'''
|
||||||
|
#
|
||||||
|
# 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
|
||||||
|
|
||||||
70
test.py
Normal file
70
test.py
Normal file
@@ -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()
|
||||||
Reference in New Issue
Block a user