예제 #1
0
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
예제 #2
0
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))
예제 #3
0
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]
예제 #5
0
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()
예제 #6
0
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
예제 #7
0
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)
예제 #8
0
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)
예제 #9
0
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)  
예제 #10
0
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;
예제 #11
0
파일: receiver.py 프로젝트: PDKK/placement
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)
예제 #12
0
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
예제 #13
0
 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()
예제 #14
0
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)
예제 #15
0
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")
예제 #16
0
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)
예제 #19
0
	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 )
예제 #20
0
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)
예제 #21
0
파일: Servo.py 프로젝트: Speedydown/IDP
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
예제 #22
0
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
예제 #23
0
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)
예제 #24
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
예제 #26
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)
예제 #27
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 = 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)
예제 #28
0
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"
예제 #29
0
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                            
예제 #30
0
파일: motor.py 프로젝트: locked/4stability
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)
예제 #31
0
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)
예제 #32
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)
예제 #33
0
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
예제 #34
0
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
예제 #35
0
파일: pi2kf.py 프로젝트: buxit/mot2bot
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)
예제 #36
0
파일: pi2kf.py 프로젝트: buxit/mot2bot
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)
예제 #37
0
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]
예제 #38
0
파일: servo.py 프로젝트: mhespenh/eva
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
예제 #39
0
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]
예제 #40
0
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)
예제 #41
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
예제 #42
0
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)
예제 #43
0
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)
예제 #44
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)
예제 #45
0
    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)
예제 #46
0
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)
예제 #47
0
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)
예제 #48
0
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))
예제 #49
0
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
예제 #50
0
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()
예제 #52
0
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)
예제 #53
0
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])
예제 #54
0
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]
예제 #55
0
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))
예제 #56
0
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))
예제 #57
0
    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