class Servo:
def __init__(self, channel, min, max, freq):
self.pwm = PWM(0x40)
self.pwm.setPWMFreq(freq)
self.channel = channel
self.min = min
self.max = max
self.range = max - min
# get middle of the range
self.current = 0
self.move_to(0.5)
def move_to(self, end_pos):
p = Process(target=move_to_process,
args=(self.pwm,
self.current,
self.channel,
self.range,
self.min,
self.max,
end_pos))
p.start()
self.current = end_pos
def get_dc_by_range(self, position):
return self.range * position + self.min
class lightChannel(object):
def __init__(self,
name,
address=None,
pwmAddr=0x41,
pwmFreq=1000,
maxintens=1.,
**kwargs):
self.name = name
self.address = address
self.maxI = maxintens
self.profileI = lightProfile(maxI=self.maxI, **kwargs)
self.pwm = PWM(pwmAddr)
self.pwm.setPWMFreq(pwmFreq)
def set_intens(self, intens):
'''
function that sets a single channels light intensity
'''
if intens > self.maxI: #protect coral from too high intensity
intens = self.maxI
if type(self.address) == list:
for a in self.address:
self.pwm.setPWM(a, 0, int(intens * 4095))
else:
self.pwm.setPWM(self.address, 0, int(intens * 4095))
class servo_control():
def setServoPulse(channel, pulse):
pulseLength = 1000000 # 1,000,000 us per second
pulseLength /= 60 # 60 Hz
pulseLength /= 4096 # 12 bits of resolution
pulse *= 1000
pulse /= pulseLength
self.pwm.setPWM(channel, 0, pulse)
def moving(self, pos):
if pos.data < 200:
#rospy.logwarn("Servo min/max values exceeded: %d. Acceptable range: 140-710" % pos.data)
pos.data = 200
elif pos.data > 550:
#rospy.logwarn("Servo min/max values exceeded: %d. Acceptable range: 140-710" % pos.data)
pos.data = 550
self.pwm.setPWM(0, 0, pos.data)
def center():
pwm = PWM(0x40)
pwm.setPWMFreq(60)
pwm.setPWM(0, 0, 345)
def __init__(self):
rospy.Subscriber("servo_position", Int16, self.moving)
self.pwm = PWM(0x40)
self.pwm.setPWMFreq(60) # Set frequency to 60 Hz
rospy.on_shutdown(center)
class Adafruit_MotorHAT:
FORWARD = 1
BACKWARD = 2
BRAKE = 3
RELEASE = 4
SINGLE = 1
DOUBLE = 2
INTERLEAVE = 3
MICROSTEP = 4
def __init__(self, addr = 0x60, freq = 1600, i2c=None, i2c_bus=None):
self._frequency = freq
self.steppers = [ Adafruit_StepperMotor(self, 1), Adafruit_StepperMotor(self, 2) ]
self._pwm = PWM(addr, debug=False, i2c=i2c, i2c_bus=i2c_bus)
self._pwm.setPWMFreq(self._frequency)
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 getStepper(self, num):
if (num < 1) or (num > 2):
raise NameError('MotorHAT Stepper must be between 1 and 2 inclusive')
return self.steppers[num-1]
def initialize():
# Initialise the PWM device using the default address
pwm = PWM(0x40)
pwm.setPWMFreq(60) # Set frequency to 60 Hz
print "Hello, I'm Ruby the Rubik's Cube Robot!"
print '\n'
F_Open()
B_Open()
L_Open()
R_Open()
sleep(2)
F_Close()
B_Close()
sleep(0.5)
F_Default()
B_Default()
sleep(1)
L_Close()
R_Close()
sleep(0.5)
L_Default()
R_Default()
sleep(1)
insert_cube()
class Motor(): servoMin = 150 # Min pulse length out of 4096 servoMax = 600 # Max pulse length out of 4096 servoInit = 185 servoMotorStart = 220 pwm = None debug = False def __init__(self, debug=True): self.debug = debug # Initialise the PWM device using the default address self.pwm = PWM(0x40, debug=debug) self.pwm.setPWMFreq(60) # Set frequency to 60 Hz self.reset() def init(self): if self.debug: print "Init...", self.pwm.setPWM(0, 0, self.servoInit) time.sleep(3) if self.debug: print "Done" def reset(self): if self.debug: print "Reset servo (minimum) %d" % self.servoMin self.pwm.setPWM(0, 0, self.servoMin) def set_speed(self, percent): if percent > 1 or percent < 0: if self.debug: print "Invalid value, must be between 0 and 1" servo_pos = int((self.servoMax - self.servoMotorStart) * percent + self.servoMotorStart) #print "Set to %.2f%% (%d)" % (percent, servo_pos) self.pwm.setPWM(0, 0, servo_pos) return servo_pos
class PanTiltDevice:
"""A class for controlling the PanTilt Device"""
pwm = PWM(0x40)
PAN_SERVO_CHANNEL = 0
TILT_SERVO_CHANNEL = 1
def __init__(self):
self.pwm = PWM(0x40)
self.pwm.setPWMFreq(10) # Set frequency to 60 Hz
# self.panServo = Servo("pan", self.pwm, self.PAN_SERVO_CHANNEL, 280, 660, 0.2, 0.003)
self.panServo = Servo("pan", self.pwm, self.PAN_SERVO_CHANNEL, 150,
700, 0.002, 0.00005)
self.tiltServo = Servo("tilt", self.pwm, self.TILT_SERVO_CHANNEL, 500,
680, 0.002, 0.00005)
def pan(self, percent):
# print "panning to %d percent" % self.panTarget
self.panServo.setTargetPosition(percent)
def tilt(self, percent):
# print "tilting to %d percent" % percent
self.tiltServo.setTargetPosition(percent)
def panAndTilt(self, panPercent, tiltPercent):
self.pan(panPercent)
self.tilt(tiltPercent)
class PanTiltDevice:
"""A class for controlling the PanTilt Device"""
pwm = PWM(0x40)
PAN_SERVO_CHANNEL = 0
TILT_SERVO_CHANNEL = 1
def __init__(self):
self.pwm = PWM(0x40)
self.pwm.setPWMFreq(10) # Set frequency to 60 Hz
# self.panServo = Servo("pan", self.pwm, self.PAN_SERVO_CHANNEL, 280, 660, 0.2, 0.003)
self.panServo = Servo("pan", self.pwm, self.PAN_SERVO_CHANNEL, 120, 700, 0.3, 0.01)
self.tiltServo = Servo("tilt", self.pwm, self.TILT_SERVO_CHANNEL, 320, 650, 0.3, 0.02)
self.panTarget = 50
self.tiltTarget = 50
self.init()
def init(self):
self.panAndTilt(self.panTarget, self.tiltTarget)
def pan(self, percent):
self.panTarget = percent
# print "panning to %d percent" % self.panTarget
self.panServo.setTargetPosition(self.panTarget)
def tilt(self, percent):
# print "tilting to %d percent" % percent
self.tiltServo.setTargetPosition(percent)
def panAndTilt(self, panPercent, tiltPercent):
self.pan(panPercent)
self.tilt(tiltPercent)
class Motor:
FRONT_LEFT = 5
FRONT_RIGHT = 4
BACK_LEFT = 3
BACK_RIGHT = 6
def __init__(self, pin_n):
#drive Output Enable in PCA low
pin = GPIO.Pin(27)
pin.write(0)
PCA9685_DEFAULT_ADDRESS = 0x40
frequency = 50
self.NAVIO_RCOUTPUT = pin_n
SERVO_MIN_ms = 1.250 # mS
SERVO_MAX_ms = 1.750 # mS
#convert mS to 0-4096 scale:
self.SERVO_MIN = math.trunc((SERVO_MIN_ms * 4096.0) / (1000.0 / frequency) - 1)
self.SERVO_MAX = math.trunc((SERVO_MAX_ms * 4096.0) / (1000.0 / frequency) - 1)
self.pwm = PWM(0x40, debug=False)
# Set frequency to 60 Hz
self.pwm.setPWMFreq(frequency)
def setSpeed(self, speed):
speed = speed if speed > self.SERVO_MIN else self.SERVO_MIN
speed = speed if speed < self.SERVO_MAX else self.SERVO_MAX
#print "Max: {}, Min: {} speed: {}".format(self.SERVO_MAX, self.SERVO_MIN, speed)
self.pwm.setPWM(self.NAVIO_RCOUTPUT, 0, speed)
def main(argv):
minValue = 0 # Min pulse length out of 4096
maxValue = 4095 # Max pulse length out of 4096
frequency = 1200 # Frequency
color = [0,0,0, 0,0,0, 0,0,0];
try:
opts, args = getopt.getopt(argv,"hr:g:b:f:a:b:c:",["red=","green=","blue=", "frequency="])
except getopt.GetoptError:
print 'setrgb.py -r <red> -g <green> -b <blue>'
sys.exit(2)
for opt, arg in opts:
if opt == '-h':
print 'setrgb.py -r <red> -g <green> -b <blue>'
sys.exit()
elif opt in ("-f", "--frequency"):
frequency = int(arg);
elif opt in ("-a", "--channela"):
color[0] = level(int(arg[:2], 16), minValue, maxValue)
color[2] = level(int(arg[2:4], 16), minValue, maxValue)
color[1] = level(int(arg[4:6], 16), minValue, maxValue)
elif opt in ("-b", "--channelb"):
color[3] = level(int(arg[:2], 16), minValue, maxValue)
color[5] = level(int(arg[2:4], 16), minValue, maxValue)
color[4] = level(int(arg[4:6], 16), minValue, maxValue)
elif opt in ("-c", "--channelc"):
color[6] = level(int(arg[:2], 16), minValue, maxValue)
color[8] = level(int(arg[2:4], 16), minValue, maxValue)
color[7] = level(int(arg[4:6], 16), minValue, maxValue)
elif opt in ("-r", "--red"):
color[0] = level(arg, minValue, maxValue)
color[3] = level(arg, minValue, maxValue)
color[6] = level(arg, minValue, maxValue)
elif opt in ("-g", "--green"):
color[2] = level(arg, minValue, maxValue)
color[5] = level(arg, minValue, maxValue)
color[8] = level(arg, minValue, maxValue)
elif opt in ("-b", "--blue"):
color[1] = level(arg, minValue, maxValue)
color[4] = level(arg, minValue, maxValue)
color[7] = level(arg, minValue, maxValue)
pwm = PWM(0x41, debug=False)
pwm.setPWMFreq(frequency) # Set frequency to 1000 Hz
i = 0;
while i < 9:
print "Setting channel %i to value %i" % (i, color[i]);
pwm.setPWM(i, 0, color[i])
i = i + 1;
class Receiver:
def __init__(self):
self.bus=smbus.SMBus(0)
self.pwm = PWM(0x40, debug=True, bus=self.bus)
self.pwm.setPWMFreq(50)
self.sensor=ColourSensor(self.bus)
def setServoPulse(self, pulse):
self.pwm.setPWM(0, 0, int(pulse * 4096 / 20))
def readSensor(self):
value=self.sensor.read()
print "R%03X G%03X B%03X" % (value['red'], value['green'], value['blue'])
if(value['blue'] <= 0x130) and (value['red'] <= 0x130):
return "Black"
elif(value['blue'] >= 0x155) and (value['red'] >= 0x155):
return "White"
else:
return "None"
def dropMarble(self):
self.setServoPulse(1.9)
sleep(1)
self.setServoPulse(1.6)
sleep(1)
def ControlProps(status):
logging.debug('ReadSensor, Thread ControlProps')
pwm = PWM(0x40)
pwm.setPWMFreq(50) # Set frequency to 60 Hz
print "Starting Control of Props according Input"
time.sleep(2)
print "Starting Loop to Control Sleep"
try:
while status["start"]:
if status["debug"]:
print("Propeller 1:" + str(status["PropValue"][0]) + " Propeller 2:" + str(status["PropValue"][1]) + " Propeller 3:" + str(status["PropValue"][2]) + " Propeller 4:" + str(status["PropValue"][3]))
time.sleep(0.1)
else:
time.sleep(0.002) #makes no sence to control prop more often with 50Hz
pwm.setPWM(0, 0, int(status["PropValue"][0]))
pwm.setPWM(1, 0, int(status["PropValue"][1]))
pwm.setPWM(2, 0, int(status["PropValue"][2]))
pwm.setPWM(3, 0, int(status["PropValue"][3]))
print "Stopping Motors"
pwm.setPWM(0, 0, 180)
pwm.setPWM(1, 0, 180)
pwm.setPWM(2, 0, 180)
pwm.setPWM(3, 0, 180)
except KeyboardInterrupt:
pwm.setPWM(0, 0, 180)
pwm.setPWM(1, 0, 180)
pwm.setPWM(2, 0, 180)
pwm.setPWM(3, 0, 180)
print "Reset Servo!!"
raise
except:
raise
return
def do_POST(self):
# Initialise the PWM device (I2C address of 0x40)
pwm = PWM(0x40, debug=True)
pwm.setPWMFreq(60)
if self.path == '/command':
form = cgi.FieldStorage(fp=self.rfile, headers=self.headers, environ={'REQUEST_METHOD':'POST'})
room = form['room'].value
name = form['name'].value
lcd.lcd_display_string("Open Blinds", 1)
location = float(form['location'].value) #% Open
print time.strftime("[%Y/%m/%d %H:%M:%S] ", time.localtime()) + "Web Command Received."
blinds = self.get_blind_data()
for blind in blinds:
if (blind.get('room') == room or blind.get('name') == name):
servoOpenLocation = float(blind.get('OpenLocation'))
servoCloseLocation = float(blind.get('CloseLocation'))
PWMLocation = int(blind.get('PWM'))
location = 100 - location
servoLocation = int(location * math.fabs(servoOpenLocation - servoCloseLocation) + servoOpenLocation)
pwm.setPWM(PWMLocation, 0, servoLocation)
time.sleep(0.50)
pwm.setPWM(PWMLocation, 0, 000)
#--- Added...may not be needed.
self.send_response(200)
self.send_header('Content-type','text-html')
self.end_headers()
self.wfile.write('Command Received and Processed')
#---
return
return self.do_GET()
def init():
global p, q, a, b, pwm, pcfADC, PGType
PGType = PGFull
# Initialise the PCA9685 PWM device using the default address
try:
pwm = PWM(0x40, debug = False)
pwm.setPWMFreq(60) # Set frequency to 60 Hz
except:
PGType = PGLite # No PCA9685 so set to Pi2Go-Lite
#use physical pin numbering
GPIO.setmode(GPIO.BOARD)
#set up digital line detectors as inputs
GPIO.setup(lineRight, GPIO.IN) # Right line sensor
GPIO.setup(lineLeft, GPIO.IN) # Left line sensor
#Set up IR obstacle sensors as inputs
GPIO.setup(irFL, GPIO.IN) # Left obstacle sensor
GPIO.setup(irFR, GPIO.IN) # Right obstacle sensor
GPIO.setup(irMID, GPIO.IN) # Centre Front obstacle sensor
#use pwm on inputs so motors don't go too fast
GPIO.setup(L1, GPIO.OUT)
p = GPIO.PWM(L1, 20)
p.start(0)
GPIO.setup(L2, GPIO.OUT)
q = GPIO.PWM(L2, 20)
q.start(0)
GPIO.setup(R1, GPIO.OUT)
a = GPIO.PWM(R1, 20)
a.start(0)
GPIO.setup(R2, GPIO.OUT)
b = GPIO.PWM(R2, 20)
b.start(0)
# Initalise the ADC
pcfADC = None # ADC object
try:
pcfADC = sgh_PCF8591P(1) #i2c, 0x48)
except:
PGType = PGLite
# initialise servos (Pi2Go-Lite only)
if PGType == PGLite:
startServos()
#set up Pi2Go-Lite White LEDs as outputs
GPIO.setup(frontLED, GPIO.OUT)
GPIO.setup(rearLED, GPIO.OUT)
#set switch as input with pullup
if PGType == PGLite:
GPIO.setup(Lswitch, GPIO.IN, pull_up_down=GPIO.PUD_UP)
else:
GPIO.setup(switch, GPIO.IN, pull_up_down=GPIO.PUD_UP)
def initialise_led():
global pwm
from Adafruit_PWM_Servo_Driver import PWM
# Set LED parameters
pwm = PWM(0x40)
pwm.setPWMFreq(1000)
print("LEDs are initialised")
def initialise_led():
global pwm
from Adafruit_PWM_Servo_Driver import PWM
# Set LED parameters
pwm = PWM(0x40)
pwm.setPWMFreq(1000)
print("LEDs are initialised")
class Robot(object):
def __init__(self, address, freq):
self.pwm = PWM(address)
self.pwm.setPWMFreq(freq)
def drive(self, left, right):
self.pwm.setPWM(0, 0, int(left))
self.pwm.setPWM(1, 0, int(right))
def run(self):
pwm = PWM(0x40, debug=True)
pwm.setPWMFreq(60) # Set frequency to 60 Hz
while True:
for i in range (0,4):
pwm.setPWM(2*i, 0, motor[i])
time.sleep(0.01)
def initializeServos(self): self.pwm = [] pwm = PWM(0x40) pwm.setPWMFreq(50) self.pwm.append(pwm) self.clearServos() pwm.setPWM(Dalek.SERVO_IRIS, 0, Dalek.SERVO_RATE_IRIS_DEFAULT) self.irisCurrentRate = Dalek.SERVO_RATE_IRIS_DEFAULT self.servoWait( Dalek.PWM_IRIS, Dalek.SERVO_IRIS, Dalek.SERVO_CRUISE_IRIS )
class AdafruitPWMOutput(Output):
def __init__(self, address = 0x40, bus = 0):
self.pwm = PWM(address, False, bus)
self.pwm.setPWMFreq(1600)
for i in range(16):
self.pwm.setPWM(i, 0, 0)
super(AdafruitPWMOutput, self).__init__()
def _setChannelValue(self, channel, value):
self.pwm.setPWM(channel, 0, value)
class Servo(object):
"""
Setting the channels, address, startingPulse and frequency to create an servo.
Using the Adafruit PWM servo driver to set the min and max Pulse of the servo.
"""
def __init__(self, address, channel, startingPulse, inverse=False, offset=0):
# Constructor
self.pulse = startingPulse
self.channel = channel
self.address = address
self.freq = 60
self.maxPulse = 600
self.minPulse = 150
self.inverse = inverse
self.offset = offset
try:
self.pwm = PWM(address)
self.pwm.setPWMFreq(self.freq)
self.setPulse(startingPulse)
except:
pass
def setPulse(self, pulse):
pulse + self.offset
# Setting the area of pulses between 150 and 600. If the input is bigger then 600 pulses
# it will be set to the max 600 pulses. If pulses are lower then 150 it will be set tot 150 pulses
if pulse > self.maxPulse:
pulse = self.maxPulse
elif pulse < self.minPulse:
pulse = self.minPulse
if self.inverse == True:
pulse = pulse - 150
#calculate difference
difference = 225 - pulse
pulse = 375 + difference
try:
self.pwm.setPWM(self.channel, 0, pulse) # 0 means: zero pulse away from input pulse.
except:
#print "moved servo " + str(self.address) + " " + str(self.channel) + " to: " + str(pulse)
pass
self.pulse = pulse # setting the input pulse
def getPulse(self):
if self.inverse == True:
pulse = self.pulse - 150
#calculate difference
difference = 225 - pulse
pulse = 375 + difference
return pulse - self.offset
return self.pulse
def init():
#-- initialization
pwm = PWM(0x40)
pwm.setPWMFreq(60) # Set frequency to 60 Hz
setServoPulse(pwm, 1, 100) #
print "delta=%d, oneDeg=%f" % (delta, oneDeg)
for i in channels:
curAngles[i] = 0
ready(pwm)
return pwm
class PanTiltDriver:
def __init__(self, customaddress=0x40, panchannel=0, tiltchannel=2):
self.Address = customaddress
self.PanChannel = panchannel
self.TiltChannel = tiltchannel
self.PanMin = 104
self.PanMax = 560
self.TiltMin = 104
self.TiltMax = 307
self.PWM = PWM(self.Address)
self.PWM.setPWMFreq(50)
def panTo(self, degrees):
if degrees < 0:
degrees = 0
if degrees > 180:
degrees = 180
ratio = degrees / float(180)
panTicks = self.PanMin + (ratio * (self.PanMax - self.PanMin))
self.PWM.setPWM(self.PanChannel, 0, int(panTicks))
def tiltTo(self, degrees):
"""
Tilts to a specified angle. Not less then 0° and no more than 90.0°
:param degrees: The amount to tilt, expressed in degrees
"""
if degrees < 0:
degrees = 0
if degrees > 90:
degrees = 90
ratio = degrees / float(90)
tiltTicks = self.TiltMin + (ratio * (self.TiltMax - self.TiltMin))
self.PWM.setPWM(self.TiltChannel, 0, int(tiltTicks))
def panLeft(self):
"""Pan completely to the left"""
self.panTo(0)
def panCenter(self):
"""Pan to center position"""
self.panTo(90)
def panRight(self):
"""Pan completely to the right"""
self.panTo(180)
def tiltDown(self):
"""Tilt completely down"""
self.tiltTo(90)
def tiltUp(self):
"""Tilt to horizontal position"""
self.tiltTo(0.0)
class Servos(object):
def __init__(self, i2cAddress, xAxisChannel, yAxisChannel, pwmFreqHz):
self.pwm = PWM(i2cAddress, debug=True)
self.pwm.setPWMFreq(pwmFreqHz)
self.xaxis = xAxisChannel
self.yaxis = yAxisChannel
def setXAxis(self, value):
self.pwm.setPWM(self.xaxis, 0, value)
def setYAxis(self, value):
self.pwm.setPWM(self.yaxis, 0, value)
class servoHandler: def __init__(self): self.frequency = 50 self.lastAddress = -1 def setServo(self, address, channel, pos): if address != self.lastAddress: self.pwm = PWM(address) # set board address # self.pwm = PWM(address, debug=True) # set board address self.pwm.setPWMFreq(self.frequency) # just to be sure, set frequency self.pwm.setPWM(channel, 0, pos) # Set the servo position time.sleep(0.3) # wait for a bit to allow for positioning
class Servos(object): def __init__(self, i2cAddress, xAxisChannel, yAxisChannel, pwmFreqHz): self.pwm = PWM(i2cAddress, debug=True) self.pwm.setPWMFreq(pwmFreqHz) self.xaxis = xAxisChannel self.yaxis = yAxisChannel def setXAxis(self, value): self.pwm.setPWM(self.xaxis, 0, value) def setYAxis(self, value): self.pwm.setPWM(self.yaxis, 0, value)
class pca9685:
def __init__(self, name):
self.name = name
self.type = 'pca9685_pwm'
self.channel_config = []
self.i2c_address = None
self.dmx_channel_start = None
self.dmx_channel_end = None
self.handler = self.default_handler
self.servo_min = 103 # Min pulse length out of 4096 = 20mS
self.servo_max = 500 # Max pulse length out of 4096
self.pwm = PWM(0x40, debug=True)
self.pwm.setPWMFreq(50) # Set frequency to 60 Hz
def __str__(self):
ret = "\nName: %s\n" % self.name
ret += "Type: %s\n" % self.type
ret += "I2C Address: %s\n" % self.i2c_address
ret += "DMX Channel Start: %d\n" % self.dmx_channel_start
ret += "DMX Channel End: %d\n" % self.dmx_channel_end
for channel_num, channel_type in enumerate(self.channel_config):
ret += "PWM Channel %d: " % channel_num
ret += "Type: %s \n" % channel_type
return ret
def default_handler(self, data):
print name, " Using default handler"
print "Data:", data
def pca9685_handler(self, data):
if controller.verbose:
print self.name, " Using hardware handler"
for channel_num, channel_type in enumerate(self.channel_config):
if channel_type == 'Dimmer':
if controller.verbose:
print "Channel:", channel_num, "Channel Type: ", channel_type, "Value: ", data[
self.dmx_channel_start + channel_num]
self.pwm.setPWM(
channel_num, 0,
data[self.dmx_channel_start + channel_num] * PCA9685_MAX /
DMX_MAX)
elif channel_type == 'Servo':
if controller.verbose:
print "Channel:", channel_num, "Channel Type: ", channel_type, "Value: ", data[
self.dmx_channel_start + channel_num]
# position is in 0-255 range
# 0degree position = 150; max degree position = 450
servo_max_delta = self.servo_max - self.servo_min
value = self.servo_min + data[self.dmx_channel_start +
channel_num] * (servo_max_delta /
DMX_MAX)
self.pwm.setPWM(channel_num, 0, value)
def feed(): pwm = PWM(0x40) pwm.setPWMFreq(50) pwm.setPWM(0, 0, 550) time.sleep(2) for x in range(0, 4): pwm.setPWM(0, 0, 128) time.sleep(0.5) pwm.setPWM(0, 0, 180) time.sleep(0.5) return "Feeding complete"
class PanTilt(object):
def __init__(self):
# Initialise the PWM device using the default address
self.pwm = PWM(0x40)
self.panMin = 80 # Min pulse length out of 4096
self.panMax = 650 # Max pulse length out of 4096
self.tiltMin = 80 # Min pulse length out of 4096
self.tiltMax = 650 # Max pulse length out of 4096
self.panChan=0
self.tiltChan=1
self.pwm.setPWMFreq(60)
# Set frequency to 60 Hz
self.pan = (self.panMin+self.panMax)/2
self.tilt = (self.tiltMin+self.tiltMax)/2
self.pwm.setPWM(self.panChan, 0, self.pan)
self.pwm.setPWM(self.tiltChan, 0, self.tilt)
time.sleep(1)
def drone(self):
# get the curses screen window
self.screen = curses.initscr()
# turn off input echoing
curses.noecho()
# respond to keys immediately (don't wait for enter)
curses.cbreak()
# map arrow keys to special values
self.screen.keypad(True)
print "Drone!"
while True:
self.char=self.screen.getch()
if self.char==113:
curses.endwin()
break
elif self.char== curses.KEY_RIGHT :
self.pan-=1
self.pwm.setPWM(self.panChan, 0, self.pan)
elif self.char== curses.KEY_LEFT :
self.pan+=1
self.pwm.setPWM(self.panChan, 0, self.pan)
elif self.char== curses.KEY_UP :
self.tilt-=1
self.pwm.setPWM(self.tiltChan, 0, self.tilt)
elif self.char== curses.KEY_DOWN :
self.tilt+=1
self.pwm.setPWM(self.tiltChan, 0, self.tilt)
else : pass
class Motor(): servoMin = 150 # Min pulse length out of 4096 servoMax = 600 # Max pulse length out of 4096 servoInit = 185 servoMotorStart = 220 speed_percent = 0 asked_speed_percent = 0 position = 0 pwm = None channel = None debug = False def __init__(self, channel, debug=True): self.channel = channel self.debug = debug # Initialise the PWM device using the default address self.pwm = PWM(0x40, debug=debug) self.pwm.setPWMFreq(60) # Set frequency to 60 Hz self.reset() def init(self): if self.debug: print "Init...", self.pwm.setPWM(self.channel, 0, self.servoInit) time.sleep(3) if self.debug: print "Done" return self def reset(self): if self.debug: print "Reset servo (minimum) %d" % self.servoMin self.pwm.setPWM(self.channel, 0, self.servoMin) def set_speed(self, percent, min=0, max=1): self.asked_speed_percent = percent if percent > 1 or percent < 0: if self.debug: print "Invalid value, must be between 0 and 1" if percent > max: percent = max if percent < min: percent = min servo_pos = int((self.servoMax - self.servoMotorStart) * percent + self.servoMotorStart) #print "Set to %.2f%% (%d)" % (percent, servo_pos) self.pwm.setPWM(self.channel, 0, servo_pos) self.position = servo_pos self.speed_percent = percent return servo_pos def adjust_speed(self, diff_speed): self.speed_percent = self.speed_percent + diff_speed self.set_speed(self.speed_percent/100.0, 0.1, 0.10)
class PanTiltDriver:
def __init__(self, customaddress=0x40, panchannel=0, tiltchannel=3):
self.Address = customaddress
self.PanChannel = panchannel
self.TiltChannel = tiltchannel
self.PanMin = 104
self.PanMax = 560
self.TiltMin = 0
self.TiltMax = 0
self.PWM = PWM(self.Address)
self.PWM.setPWMFreq(50)
def panTo(self, degrees):
if degrees < 0:
degrees = 0
if degrees > 180:
degrees = 180
ratio = degrees / float(180)
panTicks = self.PanMin + (ratio * (self.PanMax - self.PanMin))
self.PWM.setPWM(self.PanChannel, 0, int(panTicks))
def tiltTo(self, degrees):
if degrees < 0:
degrees = 0
if degrees > 90:
degrees = 90
ratio = degrees / float(90)
tiltTicks = self.TiltMin + (ratio * (self.TiltMax - self.TiltMin))
self.PWM.setPWM(self.PanChannel, 0, int(tiltTicks))
def panLeft(self):
"""Pan completely to the left"""
self.panTo(0)
def panCenter(self):
"""Pan to center position"""
self.panTo(90)
def panRight(self):
"""Pan completely to the right"""
self.panTo(180)
def tiltDown(self):
"""Tilt completely down"""
self.tiltTo(90)
def tiltUp(self):
"""Tilt to horizontal position"""
self.tiltTo(0)
def __init__(self, filter, frequency):
self.filter = filter
self.target_rotation = [0, 0]
pwm = PWM(0x40)
pwm.setPWMFreq(frequency)
self.motors = [
Motor("front_left", pwm, 4),
Motor("front_right", pwm, 5),
Motor("back_right", pwm, 6),
Motor("back_left", pwm, 7)
]
#let the motor'ss/ecu's init properly
time.sleep(3)
class ControlServer(object):
payload = {}
cycles = 0
debug = False
telem = False
def __init__(self, proto, serverIP, serverPORT):
# Init class
self.proto = proto
self.serverIP = serverIP
self.serverPORT = serverPORT
self.serverAddr = self.proto+"://"+self.serverIP+":"+self.serverPORT
self.connect()
# Init PWM Driver
self.pwm = PWM(0x40, debug=config.get('control', 'pwm_debug'))
self.pwm.setPWMFreq(config.get('control', 'pwm_freq'))
def connect(self):
# Bind socket for listening
logging.info("Binding to address: %s", self.serverAddr)
self.context = zmq.Context(1)
self.socket = self.context.socket(zmq.REP)
self.socket.bind(self.serverAddr)
def process(self):
log_every = config.get('control', 'logevery')
# Receive the payload from server
payload = None
payload = json.loads(self.socket.recv())
sequence = payload['seq']
self.cycles += 1
# Log Heartbeat
if(sequence % log_every) == 0:
logging.info("Heartbeat Alive. Sequence # %s", sequence)
# Get Telemetry data
# if(sequence % (log_every / 4)) == 0:
# self.telem = dk.update
# logging.info(self.telem)
# Send ack sequence back to server
self.socket.send(str(sequence))
payload.pop('seq', None)
if self.debug:
logging.info(payload)
return payload
class ControlServer(object):
payload = {}
cycles = 0
debug = False
telem = False
def __init__(self, proto, serverIP, serverPORT):
# Init class
self.proto = proto
self.serverIP = serverIP
self.serverPORT = serverPORT
self.serverAddr = self.proto + "://" + self.serverIP + ":" + self.serverPORT
self.connect()
# Init PWM Driver
self.pwm = PWM(0x40, debug=config.get('control', 'pwm_debug'))
self.pwm.setPWMFreq(config.get('control', 'pwm_freq'))
def connect(self):
# Bind socket for listening
logging.info("Binding to address: %s", self.serverAddr)
self.context = zmq.Context(1)
self.socket = self.context.socket(zmq.REP)
self.socket.bind(self.serverAddr)
def process(self):
log_every = config.get('control', 'logevery')
# Receive the payload from server
payload = None
payload = json.loads(self.socket.recv())
sequence = payload['seq']
self.cycles += 1
# Log Heartbeat
if (sequence % log_every) == 0:
logging.info("Heartbeat Alive. Sequence # %s", sequence)
# Get Telemetry data
# if(sequence % (log_every / 4)) == 0:
# self.telem = dk.update
# logging.info(self.telem)
# Send ack sequence back to server
self.socket.send(str(sequence))
payload.pop('seq', None)
if self.debug:
logging.info(payload)
return payload
def init():
global p, q, a, b, pwm, pcfADC, PGType
PGType = PI2KF
# Initialise the PCA9685 PWM device using the default address
try:
pwm = PWM(0x60, debug = False)
pwm.setPWMFreq(60)
except:
print "can't set PWM frequency!"
mh.getMotor(1).run(Adafruit_MotorHAT.RELEASE)
mh.getMotor(2).run(Adafruit_MotorHAT.RELEASE)
mh.getMotor(3).run(Adafruit_MotorHAT.RELEASE)
mh.getMotor(4).run(Adafruit_MotorHAT.RELEASE)
def init():
global p, q, a, b, pwm, pcfADC, PGType
PGType = PI2KF
# Initialise the PCA9685 PWM device using the default address
try:
pwm = PWM(0x60, debug = False)
pwm.setPWMFreq(60)
except:
print "can't set PWM frequency!"
mh.getMotor(1).run(Adafruit_MotorHAT.RELEASE)
mh.getMotor(2).run(Adafruit_MotorHAT.RELEASE)
mh.getMotor(3).run(Adafruit_MotorHAT.RELEASE)
mh.getMotor(4).run(Adafruit_MotorHAT.RELEASE)
class Adafruit_MotorHAT:
FORWARD = 1
BACKWARD = 2
BRAKE = 3
RELEASE = 4
LED_ON = 1
LED_OFF = 4
def __init__(self, addr = 0x60, freq = 1600):
self._i2caddr = addr # default addr on HAT
self._frequency = freq # default @1600Hz PWM freq
self.motors = [ Adafruit_DCMotor(self, m) for m in range(2) ] # modified, becausee of 2 LEDs
# for my own two servo motors
self.servoMotors = [ Jangho_Servo(self, m) for m in range(2) ]
# for my own two LEDs
self.leds = [ Jangho_LED(self, m) for m in range(2) ]
self._pwm = PWM(addr, debug=False)
self._pwm.setPWMFreq(self._frequency)
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 getLED(self, num):
if (num < 0) or (num >= 2):
raise NameError('MotorHAT LED must be between 0 and 1 inclusive')
else:
return self.leds[num]
def getServoMotor(self, num):
if (num < 0) or (num >= 2):
raise NameError('MotorHAT Servos must be between 0 and 1 inclusive')
else:
return self.servoMotors[num]
def getDCMotor(self, num):
if (num < 0) or (num >= 2):
raise NameError('MotorHAT Motor must be between 0 and 1 inclusive')
return self.motors[num]
class ServoMovementController(object):
do_shutdown = False
def __init__(self, controller_addr, servo_id, usec_max=2500, usec_min=500):
self._pwm = PWM(controller_addr, debug=True)
self._pwm.setPWMFreq(60)
self.servo_id = servo_id
self._angle = 0
self._usec_max = usec_max
self._usec_min = usec_min
self.t = threading.Thread(target=self.run, args=())
self.t.start()
def run(self):
while not self.do_shutdown or not movement_queue.empty():
if self.do_shutdown and not movement_queue.empty():
print('Received shutdown command. Waiting for all movements to complete')
movement = movement_queue.get()
self.do_it(movement)
movement_queue.task_done()
print('All movements complete. Shutting down')
def do_it(self, m):
while self._angle != m['target_angle']:
if m['target_angle'] > self._angle:
self._angle += float(m['velocity']/10.0)
if self._angle > m['target_angle']:
self._angle = m['target_angle']
if m['target_angle'] < self._angle:
self._angle -= float(m['velocity']/10.0)
if self._angle < m['target_angle']:
self._angle = m['target_angle']
self._pwm.setPWM(self.servo_id, 0, int(self.angle_to_usec(self._angle)))
time.sleep(1.0/m['velocity'])
def angle_to_usec(self, angle):
angleRange = (100 - 0)
usecRange = ( self._usec_max - self._usec_min )
angle = (((angle - 0) * usecRange) / angleRange) + self._usec_min
return angle/4
def get_cur_angle(self):
return self._angle
def shutdown(self):
self.do_shutdown = True
class Adafruit_MotorHAT:
FORWARD = 1
BACKWARD = 2
RELEASE = 4
def __init__(self, addr=0x40, freq=44.5):
self._frequency = freq
self.motors = [PWM_Motor(self, m) for m in range(16)]
self._pwm = PWM(addr, debug=False)
self._pwm.setPWMFreq(self._frequency)
def getMotor(self, num):
if (num < 0) or (num > 15):
raise NameError(
'MotorHAT Motor must be between 0 and 15 inclusive')
return self.motors[num]
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)
def main() :
pwm = PWM(0x40, debug=False) # Instantiate Adafruit PWM object
pwm.setPWMFreq( 60 ) # Set frequency to 60 Hz
keepGoing = True
while( keepGoing ):
# Change speed of continuous servo on channel O
pwm.setPWM(0, kPWM_ON, kServoLow)
time.sleep(1)
pwm.setPWM(0, kPWM_ON, kServoHi)
time.sleep(1)
# End: while( keepGoing )
sys.exit( 0 ) # Currently not reached
def main():
pwm = PWM(0x40)
servoMin = 150
servoMax = 600
pwm.setPWMFreq(60)
pwm.setPWM(0, 0, servoMin)
pwm.setPWM(1, 0, servoMin)
time.sleep(1)
pwm.setPWM(0, 0, servoMax)
time.sleep(1)
pwm.setPWM(1, 0, servoMax)
time.sleep(1)
pwm.setPWM(0, 0, servoMin)
time.sleep(1)
pwm.setPWM(1, 0, servoMin)
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)
class pca9685:
def __init__(self, name):
self.name = name
self.type = 'pca9685_pwm'
self.channel_config = []
self.i2c_address = None
self.dmx_channel_start = None
self.dmx_channel_end = None
self.handler = self.default_handler
self.servo_min = 150 # Min pulse length out of 4096
self.servo_max = 600 # Max pulse length out of 4096
self.pwm = PWM(0x40, debug=True)
self.pwm.setPWMFreq(60) # Set frequency to 60 Hz
def __str__(self):
ret = "\nName: %s\n" % self.name
ret += "Type: %s\n" % self.type
ret += "I2C Address: %s\n" % self.i2c_address
ret += "DMX Channel Start: %d\n" % self.dmx_channel_start
ret += "DMX Channel End: %d\n" % self.dmx_channel_end
for channel_num, channel_type in enumerate(self.channel_config):
ret += "PWM Channel %d: " % channel_num
ret += "Type: %s \n" % channel_type
return ret
def default_handler(self,data):
print name, " Using default handler"
print "Data:", data
def default_handler(self,data):
print self.name, " Using hardware handler"
for channel_num, channel_type in enumerate(self.channel_config):
if channel_type == 'Dimmer':
if controller.verbose:
print "Channel:", channel_num, "Channel Type: ", channel_type, "Value: ", data[self.dmx_channel_start+channel_num]
self.pwm.setPWM(channel_num, 0, data[self.dmx_channel_start+channel_num]*PCA9685_MAX/DMX_MAX)
if channel_type == 'Servo':
if controller.verbose:
print "Channel:", channel_num, "Channel Type: ", channel_type, "Value: ", data[self.dmx_channel_start+channel_num]
# position is in 0-255 range
# 0degree position = 150; max degree position = 450
servo_max_delta = self.servo_max - self.servo_min
value = self.servo_min + data[self.dmx_channel_start+channel_num] * (servo_max_delta / DMX_MAX)
if controller.verbose:
print "Channel:", channel_num, "Channel Type: ", channel_type, "Value: ", value
self.pwm.setPWM(channel_num, 0, value)
class LEDStrip(object):
# Initialize internal PWM object,
def __init__(self,
i2c_addr=0x40,
rgbw_channels=(1, 2, 3, 4),
pwm_freq=1600):
self.r_channel = rgbw_channels[0]
self.g_channel = rgbw_channels[1]
self.b_channel = rgbw_channels[2]
self.w_channel = rgbw_channels[3]
self.pwm = PWM(i2c_addr)
self.pwm.setPWMFreq(pwm_freq)
self.last_values = {
self.r_channel: None,
self.g_channel: None,
self.b_channel: None,
self.w_channel: None
}
self.set_rgbw(0, 0, 0, 0)
# Gotta clamp to [0, 1], then translate to 12-bit PWM number
def floatmap(self, intensity):
intensity = max(0.0, min(1.0, intensity))
int_tensity = int(4094 * (1.0 - intensity) + 1)
return int_tensity
# Given a channel number and an intensity in [0.0, 1.0], send the appropriate PWM command
# Only actually send the command if we're changing the int_tensity to avoid flicker
def set_channel(self, channel_num, intensity):
int_tensity = self.floatmap(intensity)
if int_tensity != self.last_values[channel_num]:
self.pwm.setPWM(channel_num, int_tensity, 0)
self.last_values[channel_num] = int_tensity
def set_rgbw(self, r, g, b, w, quiet_fool=False):
self.set_channel(self.r_channel, r)
self.set_channel(self.g_channel, g)
self.set_channel(self.b_channel, b)
self.set_channel(self.w_channel, w)
def set_color(self, color, quiet_fool=False):
self.set_channel(self.r_channel, color.r)
self.set_channel(self.g_channel, color.g)
self.set_channel(self.b_channel, color.b)
self.set_channel(self.w_channel, color.w)
class porter_car_servo_node(object):
print "porter_car_servo_node start"
def __init__(self):
print "qqqqqqqqqq"
self.node_name = rospy.get_name()
self.pwm = PWM(address=0x40, debug=False)
self.pwm.setPWMFreq(60)
self.pwm.setPWM(3, 0, 650)
self.sub_joy_ = rospy.Subscriber("joy", Joy, self.cbJoy, queue_size=1)
def cbJoy(self, joy_msg):
if (joy_msg.buttons[5] == 1):
self.pwm.setPWM(3, 0, 400)
time.sleep(0.2)
elif (joy_msg.buttons[4] == 1):
self.pwm.setPWM(3, 0, 650) #450
time.sleep(0.2)
def moveServo(direction):
#This moves a servo up or down.
# Initialise the PWM device using the default address
pwm = PWM(0x40)
# Note if you'd like more debug output you can instead run:
#pwm = PWM(0x40, debug=True)
servoMin = 150 # Min pulse length out of 4096
servoMax = 600 # Max pulse length out of 4096
pwm.setPWMFreq(60) # Set frequency to 60 Hz
if (direction=="up"):
pwm.setPWM(0, 0, servoMax)
else:
pwm.setPWM(0, 0, servoMin)
class AdafruitDriveController(DriveController):
""" Provides drive and steering control abstraction from eg PWM servo or
ESC devices. """
# TODO might need tuning or configuring
#servoMin = 150 # Min pulse length out of 4096
#servoMax = 600 # Max pulse length out of 4096
# 'standard' analog servo freq
ic_pwm_freq = 60
def __init__(self,
i2c_addr=0x40,
interface="",
hw_interface="-1",
prop_channel=0,
servo_channel=1,
debug=False):
self.debug = debug
self.prop_channel = prop_channel
self.servo_channel = servo_channel
# Initialise the PWM device
from Adafruit_PWM_Servo_Driver import PWM
# self._pwm = PWM(i2c_addr, i2c_bus=int(hw_interface), debug=debug)
self._pwm = PWM(i2c_addr, debug=debug)
self._pwm.setPWMFreq(self.ic_pwm_freq)
# Set initial positions to centre
self.set_servo_pulse(self.prop_channel, 1.5)
self.set_servo_pulse(self.servo_channel, 1.5)
def set_servo_pulse(self, channel, pulse):
""" 0.001 is ~1ms pulse so standard servo would be in range 1ms <- 1.5ms -> 2/0ms """
pulseLength = 1000000 # 1,000,000 us per second
pulseLength /= self.ic_pwm_freq # 60 Hz
if (self.debug):
logging.debug("DRIVE:\t%d us per period", pulseLength)
pulseLength /= 4096 # 12 bits of resolution
if (self.debug):
logging.debug("DRIVE:\t%d us per bit", pulseLength)
pulse *= 1000
pulse /= pulseLength
if (self.debug):
logging.debug("DRIVE:\t%d pulse sent", pulse)
self._pwm.setPWM(channel, 0, int(pulse))
class Servo:
""" Wrapper interface to a Servo."""
driver_address = 0x40
pwm_freq = 60
# The servo driver takes a 4byte number which divides the 1/F time into
# 4096 quantiles. The start of the signal and the end of the signal are
# expressed as the indices of these quantiles.
driver_resolution = 4096
def __init__(self, driver_channel, init_pos=0, move_delay=0.01):
"""
driver_channel: Provide the channel number (on the driver) of the Servo that this object represents.
init_pos: A number between 0 and 180 that specifies the initial angle
move_delay: parameter in the constructor
"""
self.move_delay = move_delay
if (init_pos < 0 or init_pos > 180):
raise ValueError("Initial position invalid:" + str(init_pos))
self.driver_channel = driver_channel
self.pwm = PWM(Servo.driver_address)
self.pwm.setPWMFreq(Servo.pwm_freq)
self.angle_to_quantile = get_pulse_lengths(Servo.pwm_freq, Servo.driver_resolution)
self.set_angle(init_pos)
def set_angle(self, angle):
if (angle < 0 or angle > 180):
raise ValueError("Destination position invalid:" + str(angle))
self.curr_pos = angle
self.pwm.setPWM(self.driver_channel, 0, self.angle_to_quantile[angle])
# Ensure that dest_angle is an int. Type check does not happen here.
def move_to(self, dest_angle):
if (dest_angle< 0 or dest_angle > 180):
raise ValueError("Destination position invalid:" + str(dest_angle))
direction = 1
if (dest_angle < self.curr_pos):
direction = -1
for angle in range(self.curr_pos, dest_angle, direction):
self.set_angle(angle)
time.sleep(self.move_delay)
self.set_angle(dest_angle)
def get_current_pos(self):
return self.curr_pos
class SubmarineMovement: def __init__(self): self.pwm = PWM(0x40, debug=True) self.servoMin = 400 self.servoMax = 600 self.servoReverse = 440 self.vertical_pin = 0 self.left_pin = 1 self.right_pin = 2 def setServoPulse(self, channel, pulse): self.pulseLength = 1000000 # 1,000,000 us per second self.pulseLength /= 60 # 60 Hz print "%d us per period" % self.pulseLength self.pulseLength /= 4096 # 12 bits of resolution print "%d us per bit" % self.pulseLength self.pulse *= 1000 self.pulse /= self.pulseLength self.pwm.setPWM(channel, 0, pulse) self.pwm.setPWMFreq(60) # Set frequency to 60 Hz def stop(self): self.pwm.setPWM(right_pin, 0, servoMin) self.pwm.setPWM(left_pin, 0, servoMin) self.pwm.setPWM(vertical_pin, 0, servoMin) def forward(self, vector): self.pwm.setPWM(left_pin, vector, servoMax) self.pwm.setPWM(right_pin, vector, servoMax) def turn(self, vector): if (vector >= 0): self.pwm.setPWM(left_pin, vector, servoMax) else: self.pwm.setPWM(right_pin, vector, servoMax) def move(self, xx, yy, zz): self.turn(xx) self.forward(yy); self.pwm.setPWM(vertical_pin, topMotor, servoMax)
def main():
pwm = PWM(0x40)
servoMin = 150
servoMax = 600
pwm.setPWMFreq(60)
RR.RobotRaconteurNode.s.NodeName = "ServoController"
servoController = servoController_imp([servoMin, servoMin], servoMin,
servoMax, pwm)
t = RR.TcpTransport()
t.StartServer(5252)
RR.RobotRaconteurNode.s.RegisterTransport(t)
autoCloseProtocol = threading.Thread(target=autoClose,
args=(
servoController,
pwm,
))
autoCloseProtocol.setDaemon(True)
autoCloseProtocol.start()
try:
with open('servo_controller_autoclose.robdef', 'r') as f:
service_def = f.read()
except:
print("error1")
try:
RR.RobotRaconteurNode.s.RegisterServiceType(service_def)
except:
print("error2")
try:
RR.RobotRaconteurNode.s.RegisterService("servcon",
"edu.rpi.controller.servcon",
servoController)
print("Connect at tcp://localhost:5252/ServoController/servcon")
raw_input("press enter to quit...\r\n")
except:
print("error")
RR.RobotRaconteurNode.s.Shutdown()
class ServoDriver:
## initializes the driver for a server
def __init__(self, address=0x40, channel=0):
self.data = []
self.server_channel = 0
self.freq = 60
self.pwm = PWM(0x40)
self.servoMin = 150
self.servoMax = 600
self.pwm.setPWMFreq(self.freq)
self.angle = 90 #centered in the beginning
self.move_t_bin = 100 #milliseconds
## sets the server to the given angle with 90 being centered
def set_angle(self, angle):
self.angle = angle
self.pwm.setPWM(self.server_channel, 0,
self.__angle_to_pulse(self.angle))
## takes care of using the server's settings to transform the values
def __angle_to_pulse(self, angle):
assert (angle >= 0 and angle <= 180)
anglePercent = angle / 180.0
return int(self.servoMin +
(self.servoMax - self.servoMin) * anglePercent)
## smooth turning to a given angle given the angular-velocity
def smooth_move(self, angle, angular_v):
constrain(angle, 0, 180)
constrain(angular_v, 0, 100)
delta_angle = abs(self.angle - angle)
t = delta_angle / angular_v * 1000 #milliseconds
def __move_delta_angle_t(self, delta_angle, t):
steps = t / self.move_t_bin
stepSize = delta_angle / steps
for step in range(0, steps):
self.angle += stepSize
self.set_angle(
angle
) # if necessary reduce processing time by working on the pulse directly
time.sleep(self.move_t_bin)
class AdafruitPwmControlledMotors:
def __init__(self, debug_flag):
self.pwmController = PWM(ADAFRUIT_PWM_ADRESS, debug = debug_flag)
self.pwmController.setPWMFreq(ADAFRUIT_PWM_FREQUENCY)
# These values must be chosen so that the ESC reacts to them.
# TODO: maybe this has to change with pwm frequency??
self.servoMin = 150 # min pulse length out of 4096
self.servoMax = 600 # max pulse length out of 4096
# channel: pin on PCA9685 where this motor is connected
# motorSpeed: desired motor speed in [0.0, 1.0]
def _servo_pulse(self, channel, motor_speed):
assert motor_speed >= 0.0
assert motor_speed <= 1.0
# TODO: check formula!
# pulse length in us (1.000.000 per second) per bit
# pulseLength = 1000000 * self.pwmFrequency / 4096
# pwmPulse = pulse * 1000 / pulseLength
pwm_pulse = motor_speed * (self.servoMax - self.servoMin) + self.servoMin
# setPWM(self, channel, on, off)
# on: The tick (between 0..4095) when the signal should transition from low to high
# off: The tick (between 0..4095) when the signal should transition from high to low
# 4096 because of 12 bit resolution
# we always start with ON at time 0
self.pwmController.setPWM(channel, 0, pwm_pulse)
# motorSpeeds: [0.0, 1.0]^4
def setSpeed(self, motor_speeds):
assert len(motor_speeds) == 4
for i in range(0,4):
assert 0.0 <= motor_speeds[i] <= 1.0
self._servo_pulse(PIN_MOTOR_1, motor_speeds[0])
self._servo_pulse(PIN_MOTOR_2, motor_speeds[1])
self._servo_pulse(PIN_MOTOR_3, motor_speeds[2])
self._servo_pulse(PIN_MOTOR_4, motor_speeds[3])
class Adafruit_MotorHAT:
FORWARD = 1
BACKWARD = 2
BRAKE = 3
RELEASE = 4
SINGLE = 1
DOUBLE = 2
INTERLEAVE = 3
MICROSTEP = 4
def __init__(self, addr=0x60, freq=1600):
self._i2caddr = addr # default addr on HAT
self._frequency = freq # default @1600Hz PWM freq
self.motors = [Adafruit_DCMotor(self, m) for m in range(4)]
self.steppers = [
Adafruit_StepperMotor(self, 1),
Adafruit_StepperMotor(self, 2)
]
self._pwm = PWM(addr, debug=False)
self._pwm.setPWMFreq(self._frequency)
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 getStepper(self, steps, num):
if (num < 1) or (num > 2):
raise NameError(
'MotorHAT Stepper must be between 1 and 2 inclusive')
return self.steppers[num - 1]
def getMotor(self, num):
if (num < 1) or (num > 4):
raise NameError('MotorHAT Motor must be between 1 and 4 inclusive')
return self.motors[num - 1]
class ServoDriver :
def __init__(self):
# PWM settings
FREQ = 60.0 # Set frequency to 60 Hz
self.pwm = PWM(0x40, debug=False)
self.pwm.setPWMFreq(FREQ)
pulseLength = 1000000 # 1,000,000 us per second
pulseLength /= FREQ # 60 Hz
pulseLength /= 4096 # 12 bits of resolution
self.pulseFactor = 1000 / pulseLength # millisecond multiplier
# Tested Servo range
self.pulsecenter = 1.6
self.pulserange = 0.8 #per adafruit 0.5 = 1.0...1.5...2.0
def move(self, servo, position):
if -1 < position < 1: #movement range -1...0...1
pulse = self.pulsecenter + (position*self.pulserange) # w/in range from middle
pulse *= self.pulseFactor # ~120-600 at 60hz
self.pwm.setPWM(servo, 0, int(pulse))
class ServoDriver:
def __init__(self):
# PWM settings
FREQ = 60.0 # Set frequency to 60 Hz
self.pwm = PWM(0x40, debug=False)
self.pwm.setPWMFreq(FREQ)
pulseLength = 1000000 # 1,000,000 us per second
pulseLength /= FREQ # 60 Hz
pulseLength /= 4096 # 12 bits of resolution
self.pulseFactor = 1000 / pulseLength # millisecond multiplier
# Tested Servo range
self.pulsecenter = 1.6
self.pulserange = 0.8 #per adafruit 0.5 = 1.0...1.5...2.0
def move(self, servo, position):
if -1 < position < 1: #movement range -1...0...1
pulse = self.pulsecenter + (position * self.pulserange
) # w/in range from middle
pulse *= self.pulseFactor # ~120-600 at 60hz
self.pwm.setPWM(servo, 0, int(pulse))
def __init__(self):
self.right_channel = [0, 1, 2]
self.left_channel = [3, 4, 5]
self.servoMin = 10 # Min pulse length out of 4096
self.servoMax = 760 # Max pulse length out of 4096
self.servoMiddle = (self.servoMin + self.servoMax) / 2.
self.tread = rospy.get_param('~tread', 0.19)
self.left_radius = rospy.get_param('~left_radius', 0.05)
self.right_radius = rospy.get_param('~right_radius', 0.05)
self.max_rad_ps = 3.8785 # 222.22222 [deg]
self.max_right_vel = self.max_rad_ps * self.right_radius
self.max_left_vel = self.max_rad_ps * self.left_radius
self.max_linear_vel = (self.max_right_vel + self.max_left_vel) / 2.
self.max_angular_vel = (self.max_right_vel -
self.max_left_vel) / self.tread
self.cmd_vel_sub = rospy.Subscriber('/cmd_vel', Twist,
self.CmdVelCallback)
# Initialise the PWM device using the default address
pwm = PWM(0x40)
pwm.setPWMFreq(60) # Set frequency to 60 Hz
