Beispiel #1
0
class PanTilt(object):
    def __init__(self, config=PanTiltConfig()):
        try:
	    self.config = config
	    if (self.config.i2c_mode == 0):
	       self.sb = open('/dev/servoblaster', 'w')
	    else:
	       self.servo16 = PWM(0x40)

        except (IOError):
            print "*** ERROR ***"
            print "Unable to communicate to the servo"
            exit()


    # Pulse width is specified in 0.01 millisec values 
    #   ie: a pulse value of 150 represents 150*.01 = 1.5 msec, which is center
    def pwm(self, channel, pulse):
	 if (self.config.i2c_mode == 0):
	    self.sb.write(str(channel) + '=' + str(int(pulse)) + '\n')
	    self.sb.flush()
	 else:
	    print "pwm(chan=", channel, ", pulse=", pulse
	    pulseLength = 1000000                   # 1,000,000 us per second
	    pulseLength /= 60                       # 60 Hz
	    pulseLength /= 4096                     # 12 bits of resolution
	    pulse *= 10                             # convert value to usec
	    pulse /= pulseLength                    # calculate channel pulse length
	    print "servo16(chan=", channel, ", pulse=", pulse
	    self.servo16.setPWM(channel, 0, int(pulse))

    def go(self, pan_angle, tilt_angle, incline_angle):
	self.pwm(self.config.tilt_pin, map(tilt_angle, 0, 100, self.config.tilt_down_limit, self.config.tilt_up_limit))
	self.pwm(self.config.pan_pin, map(pan_angle, 0, 100, self.config.pan_left_limit, self.config.pan_right_limit))
	self.pwm(self.config.incline_pin, map(incline_angle, 0, 100, self.config.incline_left_limit, self.config.incline_right_limit))
Beispiel #2
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;
Beispiel #3
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()
Beispiel #4
0
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)
Beispiel #5
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)  
 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 setAngle(axe, angle_h):
	angle = angleToPulse(angle_h)
	
	global PWM
	
	if axe == True:
		PWM.setPWM(HORIZONTAL, 0, angle)
	else:
		PWM.setPWM(VERTICAL, 0, angle)
Beispiel #8
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)
Beispiel #9
0
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
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 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
Beispiel #12
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)
Beispiel #13
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)
Beispiel #14
0
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
	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 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)
Beispiel #17
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)
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)
Beispiel #19
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
Beispiel #20
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])
Beispiel #21
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))
Beispiel #22
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
class Adafruit_MotorHAT:
	FORWARD = 1
	BACKWARD = 2
	BRAKE = 3
	RELEASE = 4
	BIPOLAR_BACKWARD = 5

	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]
Beispiel #24
0
class Servo(object):

    def __init__(self, channel, address, maxAngle=40.0, minAngle=40.0, freq=60):

        self.channel = channel
        self.address = address
        self.freq = freq
        self.maxAngle = maxAngle
        self.minAngle = minAngle
        self.maxmillisec = 2.0
        self.minmillisec = 1.0
        self.m = (self.minmillisec - self.maxmillisec)/(self.maxAngle - self.minAngle)
        self.b = self.maxmillisec - (self.m * self.minAngle)
        self.pwm = PWM()
        self.pwm.setPWMFreq(self.freq)
        self.cycle = 1.0/float(self.freq)
        self.timepertick = self.cycle / 4096

    def setAngle(self, angle):
        self.angle = angle
        print self.angle

        if self.angle > self.maxAngle:
            self.angle = self.maxAngle
        elif self.angle < self.minAngle:
            self.angle = self.minAngle
        self.millisec = self.angle * self.m + self.b
        print self.millisec

        if self.millisec > self.maxmillisec:
            self.millisec = self.maxmillisec
        elif self.millisec < self.minmillisec:
            self.millisec = self.minmillisec
        self.tickOn = (self.millisec/1000.0)/self.timepertick
        print self.tickOn
        self.pwm.setPWM(self.channel, 0, int(self.tickOn))

    def getAngle(self):
        return self.angle
Beispiel #25
0
class Transmitter:

  def __init__(self):
    self.bus=smbus.SMBus(1)
    self.pwm = PWM(0x40, debug=True, bus=self.bus)
    self.pwm.setPWMFreq(50)
    self.setServoPulse(1.5)

  def setServoPulse(self, pulse):
    self.pwm.setPWM(0, 0, int(pulse * 4096 /20))

  def send_left(self):
    self.setServoPulse(1.1)
    sleep(2)
    self.setServoPulse(1.5)
    sleep(2)

  def send_right(self):
    self.setServoPulse(1.8)
    sleep(2)
    self.setServoPulse(1.5)
    sleep(2)
Beispiel #26
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)
Beispiel #27
0
class RC(object):
  
  @staticmethod
  def map(value, fromLow, fromHigh, toLow, toHigh):
    fromRange = fromHigh - fromLow;
    toRange   = toHigh - toLow;
    return (((value - fromLow) * toRange) / fromRange) + toLow; 

  def __init__(self, servo_channel, motor_channel):
    self.servo_channel = servo_channel;
    self.motor_channel = motor_channel;
    self.servoMin      = 275; # 1ms pulse
    self.servoMax      = 550; # 2ms pulse
    self.speedMin      = 275; # 1ms pulse
    self.speedMax      = 550; # 2ms pulse
    self.pwm           = PWM(0x40);
    self.pwm.setPWMFreq(60);
    
  ######################################################################
  # Angle is in degrees. 
  # -50 degrees corresponds to full left for grasshopper
  # 50 degrees corresponds to full right for grasshopper
  #
  def setServoAngle(self, angle):
    pulse = self.map(angle, -90, 90, self.servoMin, self.servoMax);
    self.pwm.setPWM(self.servo_channel, 0, pulse);

  ##################################################################### 
  # Speed is a relative number between -2048 and 2048
  # 0     - stop
  # 2048  - full speed forward
  # -2048 - full spped backwards
  #
  # depending on surface, low numbers may not move RC car at all
  #
  def setSpeed(self, speed):
    pulse = self.map(speed * -1, -2048, 2048, self.speedMin, self.speedMax);
    self.pwm.setPWM(self.motor_channel, 0, pulse);
Beispiel #28
0
class UglyRobot(object):

  def __init__(self):
    self.pwm = PWM(0x40)
    self.pwm.setPWMFreq(60)

  def setPowerToServo(self, servo, power):
    zone = SERVO_ZONES[servo]
    if abs(power) < 0.0001:
      pwmValue = 0
    elif power > 0:
      pwmValue = zone[1] - power * (zone[1] - zone[0])
    elif power < 0:
      pwmValue = zone[2] - power * (zone[3] - zone[2])
    self.pwm.setPWM(servo, 0, int(pwmValue))

  def setVelocity(self, x, y):
    leftPower = (y + x - x * y * (sgn(x) + sgn(y)) / 2)
    rightPower = (y - x - x * y * (sgn(x) - sgn(y)) / 2)
    leftPower *= leftPower
    rightPower *= rightPower
    self.setPowerToServo(0, leftPower)
    self.setPowerToServo(1, -rightPower)
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, i2c_bus=None, 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=i2c_bus, 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))
Beispiel #30
0
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)
Beispiel #31
0
class MuleBot:
    """ Class MuleBot
  This class accepts driving commands from the keyboard and it also has
  a target mode where it drives to the target."""

    WHEEL_RADIUS = 2
    # Apparently, in the robot world, the distrance between the two motor
    # driven wheels is called wheel base length.
    WHEEL_BASE_LENGTH = 20
    SECONDS_PER_MINUTE = 60
    MAX_RPM = 12
    RADIANS_IN_CIRCLE = 2.0
    MAX_Radians_PM = RADIANS_IN_CIRCLE * MAX_RPM
    # MAX_RPS = Maximum Radians Per Second
    MAX_RPS = MAX_Radians_PM / SECONDS_PER_MINUTE
    INCHES_PER_METER = 39.3701
    CIRCUM_IN = RADIANS_IN_CIRCLE * math.pi * WHEEL_RADIUS
    CIRCUM_M = CIRCUM_IN / INCHES_PER_METER

    dcMotorPWMDurationLeft = 0
    dcMotorPWMDurationRight = 0

    def __init__(self):
        """
    __init__ initializes class variables.
    """

        global GPIO

        # running is used to control thread execution.
        self._running = True

        # Keep MuleBot parallel to the wall at this distance.
        self.distanceToWall = 0

        self.pwmEnablePin = 23  # Broadcom pin 23 was 16
        self.motor1DirectionPin = 24  # Broadcom pin 24 was 20
        self.motor2DirectionPin = 25  # Broadcom pin 25 was 21

        self.motorForward = GPIO.HIGH
        self.motorReverse = GPIO.LOW

        self.dcMotorLeftMotor = 0
        self.dcMotorRightMotor = 1

        self.laserDetectLeftPin = 6
        self.laserDetectRightPin = 5

        self.motorMaxRPM = 12.0
        self.motorMaxRadiansPM = 2 * self.motorMaxRPM

        # Pin Setup:
        GPIO.setwarnings(False)
        GPIO.setmode(GPIO.BCM)  # Broadcom pin-numbering scheme
        GPIO.setup(self.pwmEnablePin, GPIO.OUT)
        GPIO.setup(self.motor1DirectionPin, GPIO.OUT)
        GPIO.setup(self.motor2DirectionPin, GPIO.OUT)

        GPIO.output(self.pwmEnablePin, GPIO.LOW)

        # This is interupts setups.  They get used with the
        # test() method.
        #GPIO.setup(laserDetectLeftPin,  GPIO.IN, pull_up_down=GPIO.PUD_UP)
        #GPIO.setup(laserDetectRightPin, GPIO.IN, pull_up_down=GPIO.PUD_UP)

        #GPIO.add_event_detect(laserDetectLeftPin,  GPIO.FALLING, callback=myInt)
        #GPIO.add_event_detect(laserDetectRightPin, GPIO.FALLING, callback=myInt)

        # Initialise the PWM device using the default address
        self.pwm = PWM(0x40)
        # Note if you'd like more debug output you can instead run:
        #pwm = PWM(0x40, debug=True)

        #count = 1
        self.pwm.setPWMFreq(1000)  # Set frequency to 1000 Hz

        self.tgt_min_range = 15

    def terminate(self):
        """terminate is for stopping the thread."""
        self._running = False

    def mps_to_rps(self, mps):
        """
      mps_to_rps transforms meters per second to radians per second.

      @type: float
      @param: mps (meters per second)

      @rtype: float
      @param: rps (radians per second)
      """

        rps = mps * 2 / MuleBot.CIRCUM_M
        return rps

    def rps_to_mps(self, rps):
        """
      rps_to_mps transforms radians per second to meters per second.

      If rps = 2.0, then mps should equal MuleBot.CIRCUM_M because there are
      2.0 radians in a circle.

      @type: float
      @param: rps (radians per second)

      @rtype: float
      @param: mps (meters per second)
      """

        mps = rps * MuleBot.CIRCUM_M / 2
        return mps

    def rps_to_rpm(self, rps):
        """
      rps_to_rpm transforms radians per second to RPM.

      @type: float
      @param: rps (radians per second)

      @rtype: float
      @param: rpm
      """

        rpm = rps * MuleBot.SECONDS_PER_MINUTE / MuleBot.RADIANS_IN_CIRCLE
        return rpm

    def rpm_to_rps(self, rpm):
        """
      rpm_to_rps transforms RPM to radians per second.

      @type: float
      @param: rpm

      @rtype: float
      @param: rps
      """

        rps = rpm / MuleBot.SECONDS_PER_MINUTE * MuleBot.RADIANS_IN_CIRCLE
        return rps

    def rpm_to_mps(self, rpm):
        """
      rpm_to_mps transforms RPM to meters per second.

      @type: float
      @param: rpm

      @rtype: float
      @param: mps
      """

        mps = rpm / 60 * MuleBot.CIRCUM_M
        return mps

    def v(self):
        """v returns the velocity in meters per second."""

        # TODO This translation formula works, but needs simplified.

        # PWM duration can go from 0 to 4095 with 4095 representing max rpm
        #      print("MuleBot.v  MuleBot.dcMotorPWMDurationLeft:", MuleBot.dcMotorPWMDurationLeft)
        speed_percentage = float(MuleBot.dcMotorPWMDurationLeft) / 4095.0
        #      print("speed_percentage: ", speed_percentage)

        rpm = speed_percentage * self.motorMaxRPM
        #      print("rpm: ", rpm)

        secondsPerMinute = 60
        revs_per_second = rpm / secondsPerMinute
        #      print("--revs_per_second", revs_per_second)

        inches_per_rev = 2.0 * math.pi * MuleBot.WHEEL_RADIUS
        INCHES_PER_METER = 39.3701
        meters_per_rev = inches_per_rev / INCHES_PER_METER
        #      print("--meters_per_rev", meters_per_rev)

        meters_per_second = meters_per_rev * revs_per_second

        #      print("--meters_per_second: ", meters_per_second)
        return meters_per_second

    # I don't think setServoPulse is ever called.
    # Is the pulse parameter ever used?

    #servoMin = 4096 / 12  # Min pulse length out of 4096
    #servoMax = 4095       # Max pulse length out of 4096

    def setServoPulse(channel, pulse):
        """setServoPulse"""

        pulseLength = 1000000  # 1,000,000 us per second
        pulseLength /= 60  # 60 Hz
        print("%d us per period" % pulseLength)
        pulseLength /= 4096  # 12 bits of resolution
        print("%d us per bit" % pulseLength)
        pulse *= 1000
        pulse /= pulseLength
        self.pwm.setPWM(channel, 0, pulse)

    def set_wheel_drive_rates(self, v_l, v_r):
        #TODO Update this to handle nevative velocities.
        """ set_wheel_drive_rates set the drive rates of the wheels to the 
      specified velocities (rads/s).  The velocities are converted to RPM.


      @type v_l:  float
      @param v_l: velocity left wheel (rads/s)

      @type v_r: float
      @param v_r: velocity right wheel (rads/s)

      """

        # convert to rpm
        #      print(">>v_l: ", v_l)
        #      print(">>v_r: ", v_r)
        rpm_l = self.rps_to_rpm(v_l)
        rpm_r = self.rps_to_rpm(v_r)
        #      print(">>rpm_l: ", rpm_l)
        #      print(">>rpm_r: ", rpm_r)

        self.motorSpeed(rpm_l, rpm_r)
        return rpm_l, rpm_r

    def forward(self, inches):
        revolutions = inches / MuleBot.CIRCUM_IN

        rpm = MuleBot.MAX_RPM

        minutes = revolutions / rpm

        seconds = minutes * MuleBot.SECONDS_PER_MINUTE

        v = self.rpm_to_rps(rpm)
        self.set_wheel_drive_rates(v, v)

        time.sleep(seconds)
        self.stop()

    def stop(self):
        v_l = 0
        v_r = 0
        self.set_wheel_drive_rates(v_l, v_r)

    def u_turn(self, direction, diameter_in):
        """u_turn performs an 180 turn either to the 'left' or right based
        on the diameter of the turn in inches.
        
        @type: string
        @param: direction
        
        @type: float
        @type: diameter_in
        """

        #        pdb.set_trace()
        # Calculate radius of turn for the inside wheel.
        r_in = diameter_in / 2

        # Outside radius is 20 inches from inside radius.
        r_out = r_in + MuleBot.WHEEL_BASE_LENGTH

        # Outside travel distance
        travel = r_out * 3.14159
        travel_revolutions = travel / MuleBot.CIRCUM_IN

        r_ratio = r_out / r_in
        #r_ratio_half = r_ratio / 2

        speed_multiplier = MuleBot.MAX_RPM / r_ratio

        outside_rpm = r_ratio * speed_multiplier
        inside_rpm = speed_multiplier

        #
        # minutes at outside_rpm
        minutes = travel_revolutions / outside_rpm
        seconds = minutes * MuleBot.SECONDS_PER_MINUTE

        # Something isn't quite perfect.
        if direction == 'left':
            if diameter_in < 25:
                seconds -= 1
            else:
                seconds -= 2
        else:
            if diameter_in < 25:
                seconds += 1
            else:
                seconds += 2

        if direction == 'left':
            v_l = self.rpm_to_rps(inside_rpm)
            v_r = self.rpm_to_rps(outside_rpm)
        else:
            v_r = self.rpm_to_rps(inside_rpm)
            v_l = self.rpm_to_rps(outside_rpm)

        #print("2inside:   rpm: ", inside_rpm)
        #print("2outside:   rpm: ", outside_rpm)

        #print("2.1:   v_l: ", v_l)
        #print("2.1:   v_r: ", v_r)

        # Set wheel drive rates.
        self.set_wheel_drive_rates(v_l, v_r)

        # Sleep during the turn.
        time.sleep(seconds)

        # Stop
        self.stop()

        # Move forward 24 inches.
        self.forward(24)

    def u_turn_supervisor(self, command):
        __doc__ = """u_turn_supervisor parses u_turn commands and then 
        calls u_turn.
        
        Examples: 'ul10' makes a left-handed u_turn with a 10" diameter.
                   'ur40' makes a right-handed u_turn with a 40" diameter.
                        
        @type: string
        @param: command"""

        # strip the initial 'u'
        command = command[1:]
        if len(command) > 0:
            if command[0] == 'l':
                direction = 'left'
            else:
                direction = 'right'

        # strip the direction
        command = command[1:]

        if len(command) > 0:
            diameter = int(command)
            self.u_turn(direction, diameter)

    def _uni_to_diff(self, v, omega):
        """
    _uni_to_diff The is a "unicycle model".  It performs a unicycle to 
    "differential drive model" mathematical translation.

    NB: The input/output variable are in DIFFERENT units!  This is because
    the units of R are (meters/radian) does the conversion.

    This came from the 'Sobot Rimulator' by Nick McCrea.

    @type v:  float
    @param v: velocity (m/s)

    @type omega: float
    @param omega: angular velocity (rads/s)

    @rtype: float
    @return: v_l velocity left wheel (rads/s)

    @rtype: float
    @return: v_r velocity right wheel (rads/s)
    """

        #    print("--MuleBot._uni_to_diff({:.3f}, {:.3f})".format(v, omega))
        loggerMB.debug("--MuleBot._uni_to_diff({:.3f}, {:.3f})".format(
            v, omega))

        # v = translation velocity (m/s)
        # omega = angular velocity (rad/s)

        # For some reason, it is necessary to multiply the angle by -1.
        # TODO: Probably have to put this back in.
        omega *= -1.0

        inches_per_meter = 39.3701
        circumference_in = 2.0 * math.pi * MuleBot.WHEEL_RADIUS
        circumference_m = circumference_in / inches_per_meter
        radians_per_circumference = 2.0
        # R = roll?(meters/radian)
        R = circumference_m / radians_per_circumference

        # Get info in inches
        Lin = MuleBot.WHEEL_BASE_LENGTH
        # Convert inches to meters
        Lm = Lin / inches_per_meter

        # All measurements are now metric.
        v_l = ((2.0 * v) - (omega * Lm)) / (2.0 * R)
        v_r = ((2.0 * v) + (omega * Lm)) / (2.0 * R)
        loggerMB.debug(
            "--MuleBot._uni_to_diff v_l, v_r: {:.3f}, {:.3f}".format(v_l, v_r))

        rpm_l = self.rps_to_rpm(v_l)
        rpm_r = self.rps_to_rpm(v_r)
        #    print("--MuleBot._uni_to_diff rpm_l, rpm_r: {:.3f}, {:.3f}".format(rpm_l, rpm_r))
        loggerMB.debug(
            "--MuleBot._uni_to_diff rpm_l, rpm_r: {:.3f}, {:.3f}".format(
                rpm_l, rpm_r))

        return v_l, v_r

    def motorDirection(self, motorPin, direction):
        """
    motorDirection sets the direction of a single motor.
    
    Keyword arguments:
    motorPin -- Integer representing the direction pin for a specific motor.
    
    direction -- Single bit representing fowards or backwards.
    
    Usage:
        self.motorDirection(self.motor1DirectionPin, self.motorReverse)
    """
        #  print "motorPin: ", motorPin
        #  print "direction: ",  direction
        GPIO.output(motorPin, direction)

    def motorsDirection(self, direction):
        """
    motorsDirection sets the direction of both motors to the same direction.
    
    Keyword arguments:
    direction -- single character
    """

        print(direction)
        if direction == 'r' or direction == 'R':
            self.motorDirection(self.motor1DirectionPin, self.motorReverse)
            self.motorDirection(self.motor2DirectionPin, self.motorReverse)
            print("Direction reverse")
        else:
            self.motorDirection(self.motor1DirectionPin, self.motorForward)
            self.motorDirection(self.motor2DirectionPin, self.motorForward)
            print("Direction forward")

    def dcMotorLeftTurn(self, duration):
        """dcMotorLeftTurn"""

        print("From dcMotorLeftTurn: ", MuleBot.dcMotorPWMDurationLeft)
        tempPWMDurationLeft = int(MuleBot.dcMotorPWMDurationLeft * 70 /
                                  100)  # 98
        self.pwm.setPWM(self.dcMotorLeftMotor, 0, tempPWMDurationLeft)

        # Duration of the turn
        time.sleep(duration)

        # Go straight
        self.pwm.setPWM(self.dcMotorLeftMotor, 0,
                        MuleBot.dcMotorPWMDurationLeft)

    def dcMotorRightTurn(self, duration):
        """dcMotorRightTurn"""

        tempPWMDurationRight = int(MuleBot.dcMotorPWMDurationRight * 70 / 100)
        self.pwm.setPWM(self.dcMotorRightMotor, 0, tempPWMDurationRight)

        # Duration of the turn
        time.sleep(duration)

        # Go straight
        self.pwm.setPWM(self.dcMotorRightMotor, 0,
                        MuleBot.dcMotorPWMDurationRight)

    def constrainSpeed(self, speedRPM):
        """constrainSpeed ensures 0 <= speedRPM <= max.

      @type speedRPM: float
      @param speedRPM: wheel speedRPM (rpm)

      @rtype: float
      @return: constrained wheel speed (rpm)
      """

        if speedRPM > self.motorMaxRPM:
            speedRPM = self.motorMaxRPM

        if speedRPM < 0.0:
            speedRPM = 0.0

#      print ( "motorSpeed RPM adjusted: ", speedRPM )

        return speedRPM

    def motors__Direction(self, speed_l, speed_r):
        """motorDirection sets the direction of the motors based on the sign of
    the speed.

    @type: float
    @param: speed_l

    @type: float
    @param: speed_r

    """

        if speed_l >= 0:
            self.motorDirection(self.motor1DirectionPin, self.motorForward)
        else:
            self.motorDirection(self.motor1DirectionPin, self.motorReverse)

        if speed_r >= 0:
            self.motorDirection(self.motor2DirectionPin, self.motorForward)
        else:
            self.motorDirection(self.motor2DirectionPin, self.motorReverse)

    def motorSpeed(self, speedRPM_l, speedRPM_r):
        """motorSpeed sets the speed of the motors to the supplied rpm.  This has
    been updated to handle negative speeds.

    @type: float
    @param: speedRPM_l (rpm)

    @type: float
    @param: speedRPM_r (rpm)

    """

        self.motors__Direction(speedRPM_l, speedRPM_r)

        speedRPM_l = abs(speedRPM_l)
        speedRPM_r = abs(speedRPM_r)

        speedRPM_l = self.constrainSpeed(speedRPM_l)
        speedRPM_r = self.constrainSpeed(speedRPM_r)

        #   Left motor
        pwmDuration = 4095.0 * speedRPM_l / self.motorMaxRPM
        #    print("MuleBot.motorSpeed Duration left float: ", pwmDuration)
        pwmDuration = int(pwmDuration)
        #    print("MuleBot.motorSpeed Duration left int: ", pwmDuration)
        startOfPulse = 0
        self.pwm.setPWM(self.dcMotorLeftMotor, startOfPulse, pwmDuration)
        MuleBot.dcMotorPWMDurationLeft = pwmDuration

        #   Right motor
        #Adjust for right motor being faster
        pwmDuration = 4095.0 * speedRPM_r / self.motorMaxRPM
        pwmDuration = pwmDuration * 9727 / 10000  # 98.519113 percent
        pwmDuration = int(pwmDuration)
        #    print("MuleBot.motorSpeed Duration right int: ", pwmDuration)
        startOfPulse = 0
        self.pwm.setPWM(self.dcMotorRightMotor, startOfPulse, pwmDuration)
        MuleBot.dcMotorPWMDurationRight = pwmDuration

    def init(self):
        """init"""

        junk = 0
        # This is all interupt stuff for calibrating the speed
        # of the wheels.
        #self.interruptLeftCount  = -2
        #self.interruptRightCount = -2
        #self.startTimeLeft  = 0
        #self.startTimeRight = 0
        #self.lastTimeLeft   = 0
        #self.lastTimeRight  = 0

    def run1(self, _q1, _q2, _qWallDistance):
        """run1 is used to navigate the MuleBot to
       a desired distance from the wall.

       This method is a thread.

       _q1 is the current distance to the wall.
       _qWallDistance is used occasionally to establish
       the desire distance.

       _q2 is used to send steering directions to the run2 thread."""

        timeInRightTurn = 0
        timeInLeftTurn = 0

        while self._running:
            #name = threading.currentThread().getName()
            #print "Consumer thread 1:  ", name

            # This method is the only consumer of _qWallDistance.
            # Therefore checking if the queue is empty works.
            # In a multi-consumer environment, check empty()
            # can cause a race condition.
            if _qWallDistance.empty():
                pass
            else:
                self.distanceToWall = _qWallDistance.get()
                _qWallDistance.task_done()

            currentDistance = _q1.get()
            print("Current distance: ", currentDistance)

            qSize = _q1.qsize()
            if qSize > 1:
                print("***** Distance Queue Size: ", qSize, " *****")

            # Are we navigating?
            navigating = (self.distanceToWall > 0)
            if navigating:
                print("Desired distance: ", self.distanceToWall)

                accuracy = 0.5
                # Navigate
                if currentDistance < self.distanceToWall - accuracy:
                    print("Turn right >>>")
                    timeInRightTurn += 1
                    _q2.put('s1')
                elif currentDistance > self.distanceToWall + accuracy:
                    print("Turn left <<<")
                    timeInLeftTurn += 1
                    _q2.put('p1')
                else:
                    if (timeInRightTurn > 0):
                        for i in range(timeInRightTurn):
                            _q2.put('p1')
                        # Reset the time
                        timeInRightTurn = 0
                    if (timeInLeftTurn > 0):
                        for i in range(timeInLeftTurn):
                            _q2.put('s1')
                        # Reset the time
                        timeInLeftTurn = 0
                    print("On path.")
            # end if

            _q1.task_done()

    def lidarNav_queue_check(self, q_lidar_nav, tgt_range, tgt_width):
        target_range = tgt_range
        target_width = tgt_width

        # The leading 'n' has been stripped of in the run2 thread.
        if not q_lidar_nav.empty():
            command = q_lidar_nav.get()
            command = command.lower()

            first_char = 0
            if command[first_char] == 'r':
                target_range = float(command[1:])
            if command[first_char] == 'w':
                target_width = float(command[1:])

        return target_range, target_width

    def lidarNav_should_i_stay_or_should_i_go(self, tgt_range, angle):
        """lidarNav_should_i_stay_or_should_i_go will stay/stop if MuleBot is
      too close to the target.  Otherwise, it will go/continue.

      @type tgt_range: float
      @param : (inches)

      @type angle: float
      @param : (degrees)

      @rtype: float
      @return: target_range (inches)

      @rtype: float
      @return: angle (radians)
      """
        # Stop if we are too close to the target
        if tgt_range < self.tgt_min_range:
            v_l = 0
            v_r = 0
            self.set_wheel_drive_rates(v_l, v_r)

            # setting the range to zero will stop navigating.
            target_range = 0
            angle_rad = None

        else:
            # Use the updated range for the next run.
            target_range = tgt_range

            # Turn based on the angle to target.
            # Positive angles are left.
            # Negative angles are right.

            # Convert from degrees to radians.
            angle_rad = math.radians(angle)

        return target_range, angle_rad

    def velocity_check(self, v_l, v_r):
        """velocity_check slows down the velocities of the two wheels to stay
      between +-MAX_RPS. 


      @type: float
      @param: v_l (radians per second)

      @type: float
      @param: v_r (radians per second)

      @rtype: float
      @param: v_l_prime (radians per second)

      @rtype: float
      @param: v_r_prime (radians per second)

      @rtype: float
      @param: turn_duration (seconds)
      """

        if v_l == v_r:
            turn_duration = 0
            return v_l, v_r, turn_duration

        # Assumption: The robot is going forward or is stationary when it is
        #             hunting for the target, i.e., v >= 0.
        # Assumption: It is not known which direction the robot needs to turn.
        # Fact:       The return values of the _uni_to_diff method are symetrical
        #             about v.
        # Conclusion: Therefore it is only necessary to consider the larger of
        #             v_l and v_r to determine the turn duration due to the RPM
        #             limit of the motors.

        vel_max = max(v_l, v_r)
        if vel_max < MuleBot.MAX_RPS:
            turn_duration = 1
            return v_l, v_r, turn_duration


#      pdb.set_trace()
        turn_duration = vel_max / MuleBot.MAX_RPS

        v = self.v()

        # If v_? > v, v - v_? is negative. If you subract a negative, then
        # it becomes an addition.  So, it ends up on the correct side of
        # velocity.
        v_l_prime = v - (v - v_l) / turn_duration
        v_r_prime = v - (v - v_r) / turn_duration

        return v_l_prime, v_r_prime, turn_duration

    def lidarNav_turn(self, angle_rad):
        """lidarNav_turn performs a turn based on an angle.

      @type: float
      @param: angle_rad

      The return values are used for testing.

      @rtype: float
      @param: v_l (radians per second)

      @rtype: float
      @param: v_r (radians per second)

      @rtype: float
      @param: turn_duration (seconds)

      """

        #      print("--MuleBot.lidarNav_turn({:.4f}(rad))".format(angle_rad))

        #      pdb.set_trace()

        # What is our current velocity (m/s)
        v = self.v()

        rpm = self.rps_to_rpm(self.mps_to_rps(v))
        #      print("1:   rpm: ", rpm)

        # Navigate per the angle.
        omega = angle_rad

        # v_l and v_r are in radians per second
        v_l, v_r = self._uni_to_diff(v, omega)

        rpm = self.rps_to_rpm(v_l)
        #      print("2l:   rpm: ", rpm)
        rpm = self.rps_to_rpm(v_r)
        #      print("2r:   rpm: ", rpm)

        #      pdb.set_trace()
        v_l, v_r, turn_duration = self.velocity_check(v_l, v_r)

        rpm = self.rps_to_rpm(v_l)
        #      print("3l:   rpm: ", rpm)
        rpm = self.rps_to_rpm(v_r)
        #      print("3r:   rpm: ", rpm)

        self.set_wheel_drive_rates(v_l, v_r)

        # Sleep during the turn
        time.sleep(turn_duration)

        # Drive straight
        omega = 0  # zero is no turn

        # Something is wrong with _uni_to_diff.
        v_l, v_r = self._uni_to_diff(v, omega)
        # Override v_l and v_r
        v_l = self.mps_to_rps(v)
        v_r = self.mps_to_rps(v)

        rpm = self.rps_to_rpm(v_l)
        #      print("4l:   rpm: ", rpm)
        rpm = self.rps_to_rpm(v_r)
        #      print("4r:   rpm: ", rpm)
        self.set_wheel_drive_rates(v_l, v_r)

        return v_l, v_r, turn_duration

    def lidarNav(self, _q2, q_lidar_nav, q_water_pump):
        """lidarNav is used to navigate the MuleBot to
      an object.

      This method is a thread.

      _q2 is used to send steering directions to the run2 thread.
      q_lidar_nav receives target range and width information."""

        # Create the RangeBot instance.
        servo_channel = 3
        range_bot = RangeBot(servo_channel)

        UPDATE_PERIOD = .2
        MINIMUM_MANUEVER_RANGE = 28

        target_range = 0
        target_width = 0

        navigating = False
        was_navigating = False

        loggerMB.info('lidarNav before while loop.')
        while self._running:
            v = self.v()
            rpm = self.rps_to_rpm(self.mps_to_rps(v))
            #          print("---1:   rpm: ", rpm)

            loggerMB.info('lidarNav before lidarNav_queue_check.')
            target_range, target_width = \
                self.lidarNav_queue_check(q_lidar_nav, target_range, target_width)
            loggerMB.info('lidarNav after lidarNav_queue_check.')

            v = self.v()
            rpm = self.rps_to_rpm(self.mps_to_rps(v))
            #          print("---2:   rpm: ", rpm)

            # Are we navigating?
            navigating = target_range > 0 and target_width > 0
            loggerMB.debug('lidarNav navigating = {}.'.format(navigating))

            if navigating and not was_navigating:
                q_water_pump.put('won')
                was_navigating = True

            if navigating:
                #              v = self.v()
                #              print("aMuleBot.lidarNav: v (m/s): ", v)
                #              print("bMuleBot.lidarNav: target_range: ", target_range)
                #              print("cMuleBot.lidarNav: target_width: ", target_width)

                loggerMB.debug('lidarNav before execute_hunt.')
                loggerMB.debug(
                    'lidarNav before execute_hunt. range: {}, width: {}'.
                    format(target_range, target_width))
                angle, tgt_range, hits = \
                    range_bot.execute_hunt(target_range, target_width)
                loggerMB.debug('lidarNav after execute_hunt.')

                #              v = self.v()
                #              print("dMuleBot.lidarNav: v (m/s): ", v)
                #              print("eMuleBot.lidarNav: angle (deg): ", angle)
                #              print("fMuleBot.lidarNav: tgt_range (inches): ", tgt_range)

                if True:

                    loggerMB.debug(
                        'lidarNav before lidarNav_should_i_stay_or_should_i_go.'
                    )
                    target_range, angle_rad  = \
                        self.lidarNav_should_i_stay_or_should_i_go(tgt_range, angle)
                    loggerMB.debug(
                        'lidarNav after lidarNav_should_i_stay_or_should_i_go.'
                    )

                    #                  v = self.v()
                    #                  print("gMuleBot.lidarNav: v (m/s): ", v)
                    #                  print("hMuleBot.lidarNav: target_range: ", target_range)
                    #                  print("iMuleBot.lidarNav: angle_rad: ", angle_rad)
                    #                  input("Press [Enter] to continue.")

                    if target_range == 0:
                        q_water_pump.put('woff')
                        navigating = False
                        was_navigating = False

                    # Is a turn required?
                    if target_range > 0 and not (angle_rad == 0):

                        # Only make turns if target >  inches away.
                        if target_range > MINIMUM_MANUEVER_RANGE:
                            # A turn is required.

                            loggerMB.debug('lidarNav before lidarNav_turn.')
                            self.lidarNav_turn(angle_rad)

                    # end target range > 0
                # end if navigating

            time.sleep(UPDATE_PERIOD)

        loggerMB.info('lidarNav after while loop.')

    def intFromStr(self, _string, _index):
        """intFromStr extract an integer from a string."""

        list = re.findall(r'\d+', _string)
        return int(list[_index])

    def run2(self, _q2, _qWallDistance, q_lidar_nav, q_water_pump):
        """ run2 is a thread
        It is processing commands from the keyboard
        _q2 is a command queue
        _qWallDistance is the ideal distance from the wall
        q_lidar_nav is target range and width pairs"""

        while self._running:
            #                name = threading.currentThread().getName()
            #                print ("Consumer thread 2:  ", name)
            qCommand = _q2.get()
            #                print ("Here is the command... ", qCommand)
            #                print

            qSize = _q2.qsize()
            if qSize > 1:
                print("***** Command Queue Size: ", qSize, " *****")

            # Change the command to lowercase
            qCommand = qCommand.lower()
            cmd = qCommand
            command = cmd[0]

            if command == 'h':
                pass
            elif command == 'p':
                index = 0
                count = self.intFromStr(cmd, index)
                print("Left Turn, ", count, " seconds")
                self.dcMotorLeftTurn(count)
            elif command == 's':
                index = 0
                count = self.intFromStr(cmd, index)
                print("Right Turn, ", count, " seconds")
                self.dcMotorRightTurn(count)
            elif command == 't':
                self.test()
            elif command == 'u':
                # U-turn command
                self.u_turn_supervisor(cmd)
            elif command == 'w':
                q_water_pump.put(cmd)
            # n is for navigating using Lidar.
            elif command == 'n':
                if len(cmd) >= 3:
                    if cmd[1] == 'r':
                        # get range to target
                        target_range = cmd[2:]
                        target_range = int(target_range)
                        #                            print("Target range: ", target_range)
                        q_lidar_nav.put('r' + str(target_range))
                    if cmd[1] == 'w':
                        # get width of target
                        target_width = cmd[2:]
                        target_width = int(target_width)
                        #                            print("Target width: ", target_width)
                        q_lidar_nav.put('w' + str(target_width))

                # end if

            elif command == 'z':
                self.setMotorsDirection('f')

                # Get speeds (m/m)
                speeds = cmd[1:]
                comma_index = speeds.find(',')

                vmm_l = speeds[0:comma_index]
                vmm_r = speeds[comma_index + 1:]

                print("vmm_l: ", vmm_l)
                print("vmm_r: ", vmm_r)

                # Convert from meters per minute to meters per second
                v_l = float(vmm_l) / 60.0
                v_r = float(vmm_r) / 60.0

                print("v_l: ", v_l)
                print("v_r: ", v_r)

                self.set_wheel_drive_rates(v_l, v_r)

            elif command == 'f' or command == 'r':
                direction = command
                print(direction)
                self.setMotorsDirection(direction)

                index = 0
                speed = float(cmd[1:])
                print("Speed: ", speed)

                self.motorSpeed(speed, speed)
            elif command == 'd':
                index = 0
                inches = self.intFromStr(cmd, index)
                _qWallDistance.put(inches)
            else:
                print("Invalid input: ", command)
                print("Please try again.")

            #time.sleep(4)
            _q2.task_done()
        # End while

        self.shutdown()
        time.sleep(2)

    def laserNav(self, _qCommands):
        """ Name:  laserNav
          Date:  January 2018

          Arguments:  self

          Purpose:  laserNav 

      """

        lastCommandChangeTime = None
        lastCommand = None

        while self._running:

            if not (lastCommandChangeTime == None):
                if not (lastCommand == None):
                    # There is a time and command.

                    # Check if at least 30 seconds have passed since
                    # last state change.
                    TIME_TO_WAIT = 30  # Seconds
                    currentTime = time.time()  # Seconds
                    sufficientWaitTime = ((currentTime - lastCommandChangeTime)
                                          > TIME_TO_WAIT)
                    if sufficientWaitTime:
                        _qCommands.put(lastCommand)
                        lastCommandChangeTime = currentTime

            files = os.listdir("/home/pi/p/MuleBot2/")

            for file in files:
                # Looking for files ending in ".loc"
                if file.endswith(".loc"):
                    print(file)
                    command = "rm " + file
                    os.system(command)

                    # Determine which of the six states that the
                    # Laser Detector is in and steer accordingly.
                    if file == "LO_FL.loc":
                        _qCommands.put("S6")
                        lastCommand = "S6"
                    elif file == "LO_L.loc":
                        _qCommands.put("S3")
                        lastCommand = "S3"
                    elif file == "LO_C.loc":
                        _qCommands.put("S1")
                        lastCommand = "S1"
                    elif file == "RO_C.loc":
                        _qCommands.put("P1")
                        lastCommand = "P1"
                    elif file == "RO_R.loc":
                        _qCommands.put("P3")
                        lastCommand = "P3"
                    elif file == "RO_FR.loc":
                        _qCommands.put("P6")
                        lastCommand = "P6"
                    else:
                        # This should never happen.
                        print(file, " is an invalid state name")

                    # Ignoring the check for an invalid state, their
                    # there should have been a command issued.
                    lastCommandChangeTime = time.time()

            time.sleep(0.5)

    def setMotorsDirection(self, _direction):
        """setMotorsDirection sets both motors to the same direction. """

        if _direction == 'f' or _direction == 'F':
            self.motorDirection(self.motor1DirectionPin, self.motorForward)
            self.motorDirection(self.motor2DirectionPin, self.motorForward)
        elif _direction == 'r' or _direction == 'R':
            self.motorDirection(self.motor1DirectionPin, self.motorReverse)
            self.motorDirection(self.motor2DirectionPin, self.motorReverse)
        else:
            print("ERROR: setMotorsDirection bad parameter: " + direction)

    def shutdown(self):
        """shutdown """
        count = 0
        self.pwm.setPWM(0, 0, count)
        self.pwm.setPWM(1, 0, count)

        ### How to use the Enable Pin???
        #TODO:  Put this back in.
        GPIO.output(self.pwmEnablePin, GPIO.HIGH)
        GPIO.cleanup()
        print
        print("Good Bye!")
		print(Angle_B)

		for i in range(CurrentAngle, Angle_B, -1)
		    i += (200/45)
		    pwm.setPWM(pinElbow, 0, i)
		    #delay(5)
	if not isnan(Angle_C):
		# CurrentAngle = WristTilt.read()
		
		if Angle_C > CurrentAngle: 
			print(CurrentAngle)
			print(" < ")
			print(Angle_C)
			
			for i in range(CurrentAngle, Angle_C)
				pwm.setPWM(pinWrist, 0, i)
				print("New angle: ", i)
			
			

	       
    


def reset_servos():
   
   """
   Pins
   """
    # delay(2000);
    # pinFinger = 0
Beispiel #33
0
def setServoPulse(channel, pulse):
  pulseLength = 1000000                   # 1,000,000 us per second
  pulseLength /= 60                       # 60 Hz
  print "%d us per period" % pulseLength
  pulseLength /= 4096                     # 12 bits of resolution
  print "%d us per bit" % pulseLength
  pulse *= 1000
  pulse /= pulseLength
  pwm.setPWM(channel, 0, pulse)

pwm.setPWMFreq(60)                        # Set frequency to 60 Hz

def tap():
  pwm.setPWM(2, 0, 300)
    time.sleep(.5)
  pwm.setPWM(2, 0, 500)
    time.sleep(.5)
  pwm.setPWM(2, 0, 300)
    time.sleep(.5)


pwm.setPWM(1, 0, 375)
time.sleep(.5)
pwm.setPWM(2, 0, 300)
while y != "no":
  print ">>>> M value is: ", m
  m = raw_input("   Now what? >> ")
  if m != "stop" and int(m) > 149 and int(m) < 601:
    pwm.setPWM(1, 0, int(m))
    tap()
  elif int(m) < 150:
Beispiel #34
0
from Adafruit_PWM_Servo_Driver import PWM  # enabling servo driver

pwm = PWM(0x40)  # initialise the PWM device using the default address
pwm.setPWMFreq(60)  # set frequency to 60 Hz

pwm.setPWM(0, 0, 430)  # head up
#---------------------------------------------------------------
# 						Servo Drive setup
#---------------------------------------------------------------

#pwm channels
ail = 0
ele = 1
rud = 2
ttl = 3
pan = 4
tlt = 5
ttl_min = 260
#pwm setup
pwm = PWM()
pwm.setPWMFreq(43)
pwm.setPWM(ail, 1, 275)  #Aileron, RollL=200, RollR=400, Center=275
pwm.setPWM(ele, 1, 270)  #Elevator, 400 pitch down, 200 pitch up
pwm.setPWM(rud, 1, 290)  #Rudder, YawL=400, YawR=200, Center=290
pwm.setPWM(ttl, 1, ttl_min)  #Throttle
pwm.setPWM(pan, 1, 290)  #Pan, PanL=400, PanR=200, Center=290
pwm.setPWM(tlt, 1, 250)  #Tilt, TiltD=400, TileU=200, Center=250

#---------------------------------------------------------------
# 						GPIO Aknowledge/Request
#---------------------------------------------------------------

# Pin definitions
cc_akn_pin = 27
cc_req_pin = 22
auto_akn_pin = 4
auto_req_pin = 17
Beispiel #36
0
from Adafruit_PWM_Servo_Driver import PWM
import time

pwm = PWM(0x40)

pwm.setPWM(1, 0, 450)
pwm.setPWM(8, 0, 150)
time.sleep(3)
pwm.setAllPWM(0, 300)
#pwm.setPWM(1,0,300)
#pwm.setPWM(8,0,300)
Beispiel #37
0
servoMaxxx = 760 
y = "yes"
m = 1


def setServoPulse(channel, pulse):
  pulseLength = 1000000                   # 1,000,000 us per second
  pulseLength /= 60                       # 60 Hz
  print "%d us per period" % pulseLength
  pulseLength /= 4096                     # 12 bits of resolution
  print "%d us per bit" % pulseLength
  pulse *= 1000
  pulse /= pulseLength
  pwm.setPWM(channel, 0, pulse)

pwm.setPWMFreq(60)                        # Set frequency to 60 Hz

for i in range(15, 60):
  i = i * 10
  pwm.setPWM(1, 0, int(i))
  time.sleep(.5)
  print ".......", m
  m = m + 1
pwm.setPWM(1, 0, servoMin)
time.sleep(2)
pwm.setPWM(1, 0, servoMax)
time.sleep(2)
pwm.setPWM(1, 0, servoMin)
time.sleep(2)
 
Beispiel #38
0

def setServoPulse(channel, pulse):
    pulseLength = 1000000  # 1,000,000 us per second
    pulseLength /= 60  # 60 Hz
    print "%d us per period" % pulseLength
    pulseLength /= 4096  # 12 bits of resolution
    print "%d us per bit" % pulseLength
    pulse *= 1000
    pulse /= pulseLength
    pwm.setPWM(channel, 0, pulse)


pwm.setPWMFreq(60)  # Set frequency to 60 Hz
#while (True):
# Change speed of continuous servo on channel O
#pwm.setPWM(0, 0, servoMin)
#time.sleep(1)

#i = 150
#while i < 600:
#  pwm.setPWM(0, 0, i)
#  time.sleep(1)
#  i = i + 25
#  print i
#pwm.setPWM(0,0,0)

pwm.setPWM(0, 0, 250)
time.sleep(0.5)
pwm.setPWM(0, 0, 000)
Beispiel #39
0
    pulseLength = 1000000  # 1,000,000 us per second
    pulseLength /= 60  # 60 Hz
    print "%d us per period" % pulseLength
    pulseLength /= 4096  # 12 bits of resolution
    print "%d us per bit" % pulseLength
    pulse *= 1000
    pulse /= pulseLength
    pwm.setPWM(channel, 0, pulse)


pwm.setPWMFreq(60)  # Set frequency to 60 Hz

while y != "no":
    # oldcomment Change speed of continuous servo on channel O
    m = raw_input("Now what? >>")
    if m != "stop":
        pwm.setPWM(1, 0, int(m))
#  elif m == "0":
#    pwm.setPWM(1, 0, servoMinn)
#  elif m == "3":
#    pwm.setPWM(1, 0, servoMax)
#  elif m == "2":
#    pwm.setPWM(1, 0, 375)
#  elif m == "4":
#    pwm.setPWM(1, 0, servoMaxx)
#  elif m == "5":
#    pwm.setPWM(1, 0, servoMaxxx)
    else:
        y = "no"
        print "murder she wrote"
# ===========================================================================
# Example Code
# ===========================================================================

# 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 = 100  # Min pulse length out of 4096
servoMax = 800  # Max pulse length out of 4096


def setServoPulse(channel, pulse):
    """Change pulse of servo."""
    pulseLength = 1000000                   # 1,000,000 us per second
    pulseLength /= 60                       # 60 Hz
    print "%d us per period" % pulseLength
    pulseLength /= 4096                     # 12 bits of resolution
    print "%d us per bit" % pulseLength
    pulse *= 1000
    pulse /= pulseLength
    pwm.setPWM(channel, 0, pulse)

while True:
    pwm.setPWMFreq(60)                        # Set frequency to 60 Hz
    pwm.setPWM(5, 5, servoMin)
    time.sleep(1)
    pwm.setPWM(5, 5, servoMax)
    time.sleep(1)
Beispiel #41
0
import sys

sys.path.append("..")
from Navio import GPIO

#drive Output Enable in PCA low
pin = GPIO.Pin(27)
pin.write(0)

PCA9685_DEFAULT_ADDRESS = 0x40
frequency = 50

NAVIO_RCOUTPUT_1 = 3
SERVO_MIN_ms = 1.250  # mS
SERVO_MAX_ms = 1.750  # mS

#convert mS to 0-4096 scale:
SERVO_MIN = math.trunc((SERVO_MIN_ms * 4096.0) / (1000.0 / frequency) - 1)
SERVO_MAX = math.trunc((SERVO_MAX_ms * 4096.0) / (1000.0 / frequency) - 1)

pwm = PWM(0x40, debug=True)

# Set frequency to 60 Hz
pwm.setPWMFreq(frequency)

while (True):
    pwm.setPWM(NAVIO_RCOUTPUT_1, 0, SERVO_MIN)
    time.sleep(1)
    pwm.setPWM(NAVIO_RCOUTPUT_1, 0, SERVO_MAX)
    time.sleep(1)
Beispiel #42
0
import time

# ===========================================================================
# Example Code
# ===========================================================================

# 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

def setServoPulse(channel, pulse):
  pulseLength = 1000000                   # 1,000,000 us per second
  pulseLength /= 60                       # 60 Hz
  print "%d us per period" % pulseLength
  pulseLength /= 4096                     # 12 bits of resolution
  print "%d us per bit" % pulseLength
  pulse *= 1000
  pulse /= pulseLength
  pwm.setPWM(channel, 0, pulse)

pwm.setPWMFreq(60)                        # Set frequency to 60 Hz
while (True):
  # Change speed of continuous servo on channel O
  pwm.setPWM(0, 0, servoMin)
  time.sleep(1)
  pwm.setPWM(0, 0, servoMax)
Beispiel #43
0
class AdafruitMotorHAT(object):
    FORWARD = 1
    BACKWARD = 2
    RELEASE = 3

    def __init__(self, addr=0x60, freq=1600, i2c=None, i2c_bus=None):
        """
		Initialize a Motor Hat

		:param addr:
			Motor Hat address
		:type addr:
			hex
		:param freq:
			pwm frequency
		:type freq:
			int
		:param i2c:
			the i2c device
		:param i2c_bus:
			the i2c bus to utilize
		"""
        self._frequency = freq
        self.motors = [AdafruitDCMotor(self, m) for m in range(4)]
        self.steppers = [
            AdafruitStepperMotor(self, 1),
            AdafruitStepperMotor(self, 2)
        ]
        self._pwm = PWM(addr, debug=False, i2c=i2c, i2c_bus=i2c_bus)
        self._pwm.setPWMFreq(self._frequency)

    def set_pin(self, pin, value):
        """
		Set a pin output state

		:param pin:
			pin number
		:type pin:
			int
		:param value:
			pin state 0, or 1
		:type value:
			int
		:return:
			None
		"""
        if pin not in range(16):
            raise ValueError('PWM pin must be between 0 and 15 inclusive')
        if value not in [0, 1]:
            raise ValueError('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 get_stepper(self, num):
        """
		Get a stepper instance

		:param num:
			the stepper to get 1, or 2
		:type num:
			int
		:return:
			:class:`AdafruitStepperMotor'
		"""
        if num not in [1, 2]:
            raise ValueError(
                'MotorHAT Stepper must be between 1 and 2 inclusive')
        return self.steppers[num - 1]

    def get_motor(self, num):
        """
		Get a motor instance

		:param num:
			the stepper to get 1-4
		:type num:
			int
		:return:
			:class:`AdafruitDCMotor'
		"""
        if num not in [1, 2, 3, 4]:
            raise ValueError(
                'MotorHAT Motor must be between 1 and 4 inclusive')
        return self.motors[num - 1]
Beispiel #44
0
#!/usr/bin/python

from Adafruit_PWM_Servo_Driver import PWM
import time

pwm = PWM(0x40, debug=True)

cycle = 50
pwm.setPWMFreq(cycle)

pulseLength = 1000000 / cycle
tick = pulseLength / 4096  #12 bit

while (True):
    value = raw_input('Enter the micro second position: ')

    pwm.setPWM(0, 0, int(value) / tick)
    time.sleep(1)
def setServoPulse(channel, pulse):
    pulseLength = 1000000  # 1,000,000 us per second
    pulseLength /= 60  # 60 Hz
    print "%d us per period" % pulseLength
    pulseLength /= 4096  # 12 bits of resolution
    print "%d us per bit" % pulseLength
    pulse *= 1000
    pulse /= pulseLength
    pwm.setPWM(channel, 0, pulse)


pwm.setPWMFreq(60)  # Set frequency to 60 Hz
while (True):
    # Change speed of continuous servo on channel O
    pwm.setPWM(0, 0, servoMin)
    pwm.setPWM(1, 0, servoMin)
    pwm.setPWM(2, 0, servoMin)
    pwm.setPWM(3, 0, servoMin)
    pwm.setPWM(4, 0, servoMin)
    pwm.setPWM(5, 0, servoMin)
    pwm.setPWM(6, 0, servoMin)
    pwm.setPWM(7, 0, servoMin)
    pwm.setPWM(8, 0, servoMin)
    pwm.setPWM(9, 0, servoMin)
    pwm.setPWM(10, 0, servoMin)
    pwm.setPWM(11, 0, servoMin)
    pwm.setPWM(12, 0, servoMin)
    pwm.setPWM(13, 0, servoMin)
    pwm.setPWM(14, 0, servoMin)
    pwm.setPWM(15, 0, servoMin)
class Motor:

    #phase_shift = total_steps / 3

    def __init__(self, EN1, EN2, EN3, IN1, IN2, IN3):
        self.pwm = PWM(0x40)
        self.pwm.setPWMFreq(1000)
        self.EN1 = LED(EN1)
        self.EN2 = LED(EN2)
        self.EN3 = LED(EN3)
        self.IN1 = IN1
        self.IN2 = IN2
        self.IN3 = IN3
        self.pwm_values = []
        self.current_step = 0
        self.total_steps = 7200
        self.speed = 0
        self.createPWVM()
        #self.createPWM()

    def motorOff(self):
        self.EN1.off()
        self.EN2.off()
        self.EN3.off()
        self.pwm.setPWM(self.IN1, 0, 0)
        self.pwm.setPWM(self.IN2, 0, 0)
        self.pwm.setPWM(self.IN3, 0, 0)

    def motorSpeed(self, speed):
        self.speed = speed

    def motorStep(self):
        self.current_step = (self.current_step + self.speed) % self.total_steps
        current_stepA = self.current_step
        current_stepB = (current_stepA +
                         self.total_steps / 3) % self.total_steps
        current_stepC = (current_stepA +
                         self.total_steps / 3 * 2) % self.total_steps
        self.EN1.on()
        self.EN2.on()
        self.EN3.on()
        self.pwm.setPWM(self.IN1, 0, self.pwm_values[current_stepA])
        self.pwm.setPWM(self.IN2, 0, self.pwm_values[current_stepB])
        self.pwm.setPWM(self.IN3, 0, self.pwm_values[current_stepC])

    def createPWVM(self):
        max_amplitude = 4721
        divisor = 1
        for step in range(0, self.total_steps):
            degree = step * 360.0 / self.total_steps
            phaseA = math.sin(
                degree * math.pi / 180.0) * max_amplitude / divisor
            phaseB = math.sin(
                (degree - 120) * math.pi / 180.0) * max_amplitude / divisor
            phaseC = math.sin(
                (degree + 120) * math.pi / 180.0) * max_amplitude / divisor

            voff = (min(phaseA, phaseB, phaseC) +
                    max(phaseA, phaseB, phaseC)) / 2
            self.pwm_values.append((int((phaseA - voff) / 2)) +
                                   (2047 / divisor))

    def createPWM(self):
        max_amplitude = 4095
        divisor = 1
        for step in range(0, self.total_steps):
            degree = step * 360.0 / self.total_steps
            phaseA = math.sin(
                degree * math.pi / 180.0) * max_amplitude / divisor

            self.pwm_values.append(int(phaseA) + (2047 / divisor))
Beispiel #47
0
#      random_second_number = random.randint(0, 1000)
#      random_third_number = random.randint(0, 1000)
#      pwm.setPWM(1, 0, random_number)            # changed 0 to 13 to 1
#      pwm.setPWM(2, 0, random_second_number)     # changed 0 to 14 to 2
#      pwm.setPWM(3, 0, random_third_number)      # changed 0 to 15 to 3
#
#      time.sleep(0.2)                                  #waits a fifth second
#      pwm.setPWM(1, 0, 0)    # changed 0 to 13 to 1
#      pwm.setPWM(2, 0, 0)    # changed 0 to 14 to 2
#      pwm.setPWM(3, 0, 0)    # changed 0 to 15 to 3

pwm = PWM(0x40)
for x in range(1, 2):  #perform the while loop 1 times
    new_number = 1
    while new_number < 17:
        pwm.setPWM(new_number, 0, 500)
        new_number = new_number + 1
        time.sleep(
            0.05
        )  # waits a twentieth second. change frequency: p.ChangeFrequency(new frequency in Hz)
pwm = PWM(0x41)
for x in range(1, 2):  #perform the while loop 1 times
    new_number_2 = 1
    while new_number_2 < 17:
        pwm.setPWM(new_number_2, 0, 500)
        new_number_2 = new_number_2 + 1
        time.sleep(
            0.05
        )  # waits a twentieth second. change frequency: p.ChangeFrequency(new frequency in Hz)
pwm = PWM(0x42)
for x in range(1, 2):  #perform the while loop 1 times
# ===========================================================================
# Example Code
# ===========================================================================

# 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

def setServoPulse(channel, pulse):
  pulseLength = 1000000                   # 1,000,000 us per second
  pulseLength /= 60                       # 60 Hz
  print "%d us per period" % pulseLength
  pulseLength /= 4096                     # 12 bits of resolution
  print "%d us per bit" % pulseLength
  pulse *= 1000
  pulse /= pulseLength
  pwm.setPWM(channel, 0, pulse)

pwm.setPWMFreq(50)                        # Set frequency to 60 Hz
pwm.setPWM(0, 0,305)
pwm.setPWM(1, 0,235)
pwm.setPWM(2, 0,520)
pwm.setPWM(3, 0,305)
pwm.setPWM(4, 0,305)
pwm.setPWM(5, 0,340)
Beispiel #49
0
class RGB_LED(object):
    """Object communicating to the LEDs.

        Low level class that creates the PWM messages that are sent to the
        microcontroller. It contains offset addresses relatives to the
        address of the various LEDs.

        Each LED on a Duckiebot or a watchtower is indexed by a number:

        +------------------+------------------------------------------+
        | Index            | Position (rel. to direction of movement) |
        +==================+==========================================+
        | 0                | Front left                               |
        +------------------+------------------------------------------+
        | 1                | Rear left                                |
        +------------------+------------------------------------------+
        | 2                | Top / Front middle                       |
        +------------------+------------------------------------------+
        | 3                | Rear right                               |
        +------------------+------------------------------------------+
        | 4                | Front right                              |
        +------------------+------------------------------------------+

        Setting the color of a single LED is done by setting the brightness of the
        red, green, and blue channels to a value between 0 and 255. The communication
        with the hardware controller is abstracted through the :obj:`setRGB` method. By
        using it, you can set directly set the desired color to any LED.

    """

    # Class-specific constants
    OFFSET_RED = 0  #: Offset address for the red color
    OFFSET_GREEN = 1  #: Offset address for the green color
    OFFSET_BLUE = 2  #: Offset address for the blue color

    def __init__(self, debug=False):
        self.pwm = PWM(address=0x40, debug=debug)
        for i in range(15):
            # Sets fully off all the pins
            self.pwm.setPWM(i, 0, 4095)

    def setLEDBrightness(self, led, offset, brightness):
        """Sets value for brightness for one color on one LED.

            Calls the function pwm.setPWM to set the PWM signal according to
            the input brightness.

            Typically shouldn't be used directly. Use :obj:`setRGB` instead.

            Args:
                led (:obj:`int`): Index of specific LED (from the table above)
                offset (:obj:`int`): Offset for color
                brightness (:obj:`int8`): Intensity of brightness (between 0 and 255)

        """
        self.pwm.setPWM(3 * led + offset, brightness << 4, 4095)

    def setRGB(self, led, color):
        """Sets value for brightness for all channels of one LED

            Converts the input color brightness from [0,1] to [0,255] for all
            channels, then calls self.setLEDBrightness with the right offset
            corresponding to the color channel in the PWM signal and the color
            value as int8.

            Args:
                led (:obj:`int`): Index of specific LED (from the table above)    
                color (:obj:`list` of :obj:`float`): Brightness for the three RGB channels, in interval [0,1]
        """

        self.setLEDBrightness(led, self.OFFSET_RED, int(color[0] * 255))
        self.setLEDBrightness(led, self.OFFSET_GREEN, int(color[1] * 255))
        self.setLEDBrightness(led, self.OFFSET_BLUE, int(color[2] * 255))

    def __del__(self):
        """Destructur method.

            Turns off all the LEDs and deletes the PWM object.

        """
        for i in range(15):
            # Sets fully off all channels of all the LEDs (3 channles * 5 LEDs)
            self.pwm.setPWM(i, 0, 4095)
        del self.pwm
Beispiel #50
0
    3500,
    3600,
    3700,
    3800,
    3900,
    4000,
    4095
]

# PCA9685-PWM-Board
pwm = PWM(0x40)
pwm.setPWMFreq(400)
PWM_CHANNELS = 16
channels = []
for i in range(PWM_CHANNELS):
    pwm.setPWM(i, 0, 0)
    channels.append(0)

led = LED_RED


def gpio_callback(gpio_id, val):
    print("gpio %s: %s" % (gpio_id, val))
    if gpio_id == 10:
        if val:
            pwm.setPWM(1, 0, 0)
        else:
            pwm.setPWM(1, 0, 4000)


def socket_callback(socket, val):
Beispiel #51
0
def main():
    pwm = PWM(0x40)
    pwm.setPWMFreq(100)
    vs = PiVideoStream(resolution=(640, 480)).start()
    time.sleep(1.0)
    frame = vs.read()
    prvs = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
    hsv = np.zeros_like(frame)
    hsv[..., 1] = 255
    mode = 0
    speed = 0
    steer = 0

    while True:
        frame = vs.read()
        #frame = rotate_image(frame)
        next = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
        if mode == 3:
            flow = cv2.calcOpticalFlowFarneback(prvs, next, 0.5, 3, 15, 3, 5,
                                                1.2, 0)
            mag, ang = cv2.cartToPolar(flow[..., 0], flow[..., 1])
            hsv[..., 0] = ang * 180 / 3.14 / 2
            hsv[..., 2] = cv2.normalize(mag, None, 0, 255, cv2.NORM_MINMAX)
            bgr = cv2.cvtColor(hsv, cv2.COLOR_HSV2BGR)

        cv2.putText(frame, "Speed: {}, Steer: {}".format(speed, steer),
                    (10, 20), cv2.FONT_HERSHEY_PLAIN, 1.0, (255, 0, 0))
        if mode == 1:
            frame = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
        if mode == 2:
            frame = cv2.Canny(frame, 20, 100)
        if mode == 3:
            cv2.imshow("Frame", bgr)
        else:
            cv2.imshow("Frame", frame)

        key = cv2.waitKey(1) & 0xFF
        if key == ord("q"):
            break
        if key == ord("f"):
            mode = 3
        if key == ord("e"):
            mode = 2
        if key == ord("g"):
            mode = 1
        if key == ord("c"):
            mode = 0
        if key == ord("l"):
            pwm.setPWM(0, 0, 500)
        if key == ord("r"):
            pwm.setPWM(0, 0, 300)
        if key == 81:  # left
            if steer > -1:
                steer = steer - 0.1
        if key == 83:  # right
            if steer < 1:
                steer = steer + 0.1
        if key == 82:  # up
            if speed < 1:
                speed = speed + 0.1
        if key == 84:  # down
            if steer > -1:
                speed = speed - 0.1
        if key == ord("s"):
            speed = 0
            steer = 0
        pwm.setPWM(0, 0, 500 + int(speed * 100))
        pwm.setPWM(2, 0, 670 - int(steer * 100))
        prvs = next

    cv2.destroyAllWindows()
    vs.stop()
Beispiel #52
0
    elif a < -40.0:
        a = -40.0
    print a
    ms = a * m + b
    if ms > 2.0:
        ms = 2.0
    elif ms < 1.0:
        ms = 1.0
    s = ms / 1000
    print s
    ticks = int(s / timepertick)
    print ticks
    return ticks


pwm.setPWM(0, 0, AngleToPulse(0))
time.sleep(1)
pwm.setPWM(1, 0, AngleToPulse(0))
time.sleep(1)
pwm.setPWM(2, 0, AngleToPulse(0))
time.sleep(1)

while (True):
    # Change speed of continuous servo on channel O
    a = float(raw_input("Angle -40 to 40: "))
    pwm.setPWM(2, 0, AngleToPulse(a))
    #print 1
    #time.sleep(1)
    #setServoPulse(0, 50)
    #print 50
    #time.sleep(1)
Beispiel #53
0
    print "d% us per bit" % pulseLength
    pulse *= 1000
    pulse /= pulseLength
    pwm.setPWM(channel, 0, pulse)


for frame in camera.capture_continuous(rawCapture,
                                       format='bgr',
                                       use_video_port=True):
    image = frame.array

    pwm.setPWMFreq(60)
    print "---------"
    #change speed of cont servo on channel 0

    pwm.setPWM(LEFT_RIGHT, 0, MAX_LEFT)
    pwm.setPWM(UP_DOWN, 0, 550)
    time.sleep(2)

    pwm.setPWM(LEFT_RIGHT, 0, 541)
    pwm.setPWM(UP_DOWN, 0, 550)
    time.sleep(2)

    pwm.setPWM(LEFT_RIGHT, 0, 503)
    pwm.setPWM(UP_DOWN, 0, 550)
    time.sleep(2)

    pwm.setPWM(LEFT_RIGHT, 0, 465)
    pwm.setPWM(UP_DOWN, 0, 550)

    cv2.imshow("Frame", image)
Beispiel #54
0
from Adafruit_PWM_Servo_Driver import PWM
import time

pwm1 = PWM(0x40)
pwm2 = PWM(0x41)
pwm1.setPWMFreq(60)
pwm2.setPWMFreq(60)

mid = 300
#end = 590

for channel in range(0, 6):
    pwm2.setPWM(channel, 0, mid)
    time.sleep(0.5)

for channel in range(8, 14):
    pwm2.setPWM(channel, 0, mid)
    time.sleep(0.5)

for channel in range(0, 6):
    pwm1.setPWM(channel, 0, mid)
    time.sleep(0.5)
Beispiel #55
0
class CarControl(object):
    def __init__(self):
        self._pwm = PWM(0x40)  # I2C address
        self._pwm.setPWMFreq(SERVO_REFRESH_FREQUENCY)
        self.motor_stop_and_center()
        self.reset_steering()
        return

    def more_gas(self):
        global gas_pedal
        if gas_pedal <= MOTOR_MAX_THROTTLE_PERCENTAGE:
            gas_pedal += MOTOR_STEP_SIZE_PERCENT
        else:
            gas_pedal = MOTOR_MAX_THROTTLE_PERCENTAGE
        self.throttle = gas_pedal
        return

    def reverse(self):
        global gas_pedal
        if gas_pedal >= MOTOR_MAX_REVERSE_PERCENTAGE:
            gas_pedal -= MOTOR_STEP_SIZE_PERCENT
        else:
            gas_pedal = MOTOR_MAX_REVERSE_PERCENTAGE
        self.throttle = gas_pedal
        return

    def active_braking(self):
        global gas_pedal
        # ACTIVE BRAKE OPPOSITE DIRECTION
        if gas_pedal > 0:
            self.throttle = -MOTOR_BRAKING_FORCE
        elif gas_pedal < 0:
            self.throttle = MOTOR_BRAKING_FORCE
        time.sleep(MOTOR_BRAKING_DELAY)
        self.throttle = 0
        gas_pedal = 0
        return

    def sensor_braking(self):
        global gas_pedal
        # ACTIVE BREAK OPPOSITE DIRECTION
        self.throttle = -MOTOR_BRAKING_FORCE
        time.sleep(MOTOR_BRAKING_DELAY)
        self.throttle = 0
        gas_pedal = 0
        return

    def _get_motor_pwm_value(self):
        if self.throttle == 0:
            return MOTOR_STOP_PWM_VALUE
        elif self.throttle < 0:
            coeff = -self.throttle / 100.0
            min_value = MOTOR_DEADBAND_START_VALUE
            max_value = MOTOR_MIN_PWM_VALUE
        else:
            coeff = self.throttle / 100.0
            min_value = MOTOR_DEADBAND_STOP_VALUE
            max_value = MOTOR_MAX_PWM_VALUE
        assert 0.0 <= coeff <= 1.0
        diff = max_value - min_value
        # Interpolate between the min and max values
        return int(round(min_value + coeff * diff, 0))

    @property
    def throttle(self):
        return self._throttle

    @throttle.setter
    def throttle(self, value):
        assert -100 <= value <= 100
        self._throttle = value
        pwm_value = self._get_motor_pwm_value()
        print('Setting motor PWM to', pwm_value)
        self._pwm.setPWM(MOTOR_PWM_CHANNEL, 0, pwm_value)
        return

    def motor_stop_and_center(self):
        global currentSteeringAngle
        self.throttle = 0
        currentSteeringAngle = steerCenter
        return

    def stop_and_reset(self):
        self.throttle = 0
        self.active_breaking()
        self.motor_stop_and_center()
        self.reset_steering()
        return

    def reset_steering(self):
        global currentSteeringAngle
        self._pwm.setPWM(STEERING_PWM_CHANNEL, 0, steerCenter)
        currentSteeringAngle = steerCenter
        return

    def steer_right(self):
        global currentSteeringAngle
        if currentSteeringAngle <= (steerRight - STEERING_ANGLE_STEP):
            currentSteeringAngle += STEERING_ANGLE_STEP
        else:
            currentSteeringAngle = steerRight
        self._pwm.setPWM(STEERING_PWM_CHANNEL, 0, currentSteeringAngle)
        return

    def steer_left(self):
        global currentSteeringAngle
        if currentSteeringAngle >= (steerLeft + STEERING_ANGLE_STEP):
            currentSteeringAngle -= STEERING_ANGLE_STEP
        else:
            currentSteeringAngle = steerLeft
        self._pwm.setPWM(STEERING_PWM_CHANNEL, 0, currentSteeringAngle)
        return
Beispiel #56
0
# ===========================================================================
# Hatalogico PWM Blanking Test for LEDs - powered by Adafruit's Libraries
# -------------------------------------------------
# Date: 24/3/2015
# Stolen By: John Lumley
#
# BIG THANKS TO ADAFRUIT INDUSTRIES FOR MAKING THIS POSSIBLE AND EASY
# ===========================================================================

import os, sys

# DETERMINE CURRENT PATH
scriptPath = os.path.realpath(os.path.dirname(sys.argv[0]))
os.chdir(scriptPath)
# APPEND FOLDER OF REQUIRED LIBRARY
sys.path.append("Adafruit/Adafruit_PWM_Servo_Driver")
# FINALLY LOAD THE LIBRARY
from Adafruit_PWM_Servo_Driver import PWM

# SETUP THE PWM DRIVER (I2C ADDRESS IS 70 BY DEFAULT ON HATALOGICO)
# EACH CHANNEL RANGES FROM 0 (off) TO 4095 (on)
pwm = PWM(0x70)
# SET FREQUENCY
pwm.setPWMFreq(120)

# LOOP THROUGH THE 16 PINS - note: range goes up to but does not include the 2nd parameter
for pwmPin in range(0, 16):
	# TURN IT OFF COMPLETELY
	pwm.setPWM(pwmPin, 0, 0)
Beispiel #57
0
class Adafruit_MotorShield:

    LOW = 0
    HIGHT = 1

    def __init__(self, address=0x60, debug=False):
        self.address = address
        self.pwm = PWM(address, debug)
        self.steppers = [
            Adafruit_StepperMotor(debug),
            Adafruit_StepperMotor(debug)
        ]
        self.dcmotors = [
            Adafruit_DCMotor(debug),
            Adafruit_DCMotor(debug),
            Adafruit_DCMotor(debug),
            Adafruit_DCMotor(debug)
        ]

    def begin(self, freq=1600):
        self.pwm.begin()
        self.freq = freq
        self.pwm.setPWMFreq(freq)
        # This is the maximum PWM frequency
        for i in range(0, 16):
            self.pwm.setPWM(i, 0, 0)

    def setPWM(self, pin, value):
        if value > 4095:
            self.pwm.setPWM(pin, 4096, 0)
        else:
            self.pwm.setPWM(pin, 0, value)

    def setPin(self, pin, value):
        if value == self.LOW:
            self.pwm.setPWM(pin, 0, 0)
        else:
            self.pwm.setPWM(pin, 4096, 0)

    def getMotor(self, num):
        if num > 4:
            return

        num -= 1
        if (self.dcmotors[num].motornum is None):
            # not init'd yet!
            self.dcmotors[num].motornum = num
            self.dcmotors[num].MC = self
            if (num == 0):
                pwm = 8
                in2 = 9
                in1 = 10
            elif (num == 1):
                pwm = 13
                in2 = 12
                in1 = 11
            elif (num == 2):
                pwm = 2
                in2 = 3
                in1 = 4
            elif (num == 3):
                pwm = 7
                in2 = 6
                in1 = 5
            self.dcmotors[num].PWMpin = pwm
            self.dcmotors[num].IN1pin = in1
            self.dcmotors[num].IN2pin = in2

        return self.dcmotors[num]

    def getStepper(self, steps, num):
        if num > 2:
            return

        num -= 1
        if self.steppers[num].steppernum is None:
            # not init'd yet!
            self.steppers[num].steppernum = num
            self.steppers[num].revsteps = steps
            self.steppers[num].MC = self

            if num == 0:
                pwma = 8
                ain2 = 9
                ain1 = 10
                pwmb = 13
                bin2 = 12
                bin1 = 11
            elif num == 1:
                pwma = 2
                ain2 = 3
                ain1 = 4
                pwmb = 7
                bin2 = 6
                bin1 = 5
            self.steppers[num].PWMApin = pwma
            self.steppers[num].PWMBpin = pwmb
            self.steppers[num].AIN1pin = ain1
            self.steppers[num].AIN2pin = ain2
            self.steppers[num].BIN1pin = bin1
            self.steppers[num].BIN2pin = bin2

        return self.steppers[num]
Beispiel #58
0
import time

# ===========================================================================
# Example Code
# ===========================================================================

# 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 = 104  # Min pulse length out of 4096
servoMid = 270
servoMax = 380  # Max pulse length out of 4096


pwm.setPWMFreq(50)                        # Set frequency to 50 Hz
while (True):
  # Change speed of continuous servo on channel O
  pwm.setPWM(3, 0, servoMin)
  time.sleep(1)
  pwm.setPWM(3, 0, servoMid)
  time.sleep(1)
  pwm.setPWM(3, 0, servoMax)
  time.sleep(1)
  pwm.setPWM(3, 0, servoMid)
  time.sleep(1)



Beispiel #59
0
class RoboCarModel1(RoboCarBase.RoboCarBase):
    model = "RobotCar Model 1"
    version = 0.1

    def stop(self):
        self.mh.getMotor(1).run(Adafruit_MotorHAT.RELEASE)
        self.mh.getMotor(3).run(Adafruit_MotorHAT.RELEASE)

    def __init__(self, i2c_addr):
        super(RoboCarModel1, self).__init__()
        #logging.basicConfig(stream=sys.stderr, level=logging.DEBUG)
        self.i2c_addr = i2c_addr
        self.mh = Adafruit_MotorHAT(addr=i2c_addr)
        self.pwm = PWM(0x60)
        self.servoMin = 150  # Min pulse length out of 4096
        self.servoMax = 600  # Max pulse length out of 4096
        self.pwm.setPWMFreq(60)
        self.motorLeft = self.mh.getMotor(3)
        self.motorRight = self.mh.getMotor(1)
        atexit.register(self.stop)
        self.isRunningForward = 0
        self.isRunningReverse = 0
        self.isRunningLeft = 0
        self.isRunningRight = 0

    def setCamAngle(self, angle):
        self.camAngle = angle
        newAngle = self.map(self.camAngle, 0, 180, self.servoMin,
                            self.servoMax)
        self.pwm.setPWM(0, 0, newAngle)

    def setMotors(self, motorLeftSpeed, motorRightSpeed, motorLeftDir,
                  motorRightDir):
        # set the speed to start, from 0 (off) to 255 (max speed)
        newLeftSpeed = self.map(motorLeftSpeed, 0, 10, 0, 255)
        newRightSpeed = self.map(motorRightSpeed, 0, 10, 0, 255)
        self.motorLeft.setSpeed(newLeftSpeed)
        self.motorRight.setSpeed(newRightSpeed)
        self.motorLeft.run(motorLeftDir)
        self.motorRight.run(motorRightDir)

    def setMotorForward(self):
        self.isRunningForward = 1
        self.isRunningReverse = 0
        self.setMotors(self.motorSpeed, self.motorSpeed,
                       Adafruit_MotorHAT.FORWARD, Adafruit_MotorHAT.FORWARD)

    def setMotorReverse(self):
        self.isRunningForward = 0
        self.isRunningReverse = 1
        self.setMotors(self.motorSpeed, self.motorSpeed,
                       Adafruit_MotorHAT.BACKWARD, Adafruit_MotorHAT.BACKWARD)

    def setMotorForwardReverseStop(self):
        self.isRunningForward = 0
        self.isRunningReverse = 0
        if (self.isRunningLeft):
            self.setMotorLeft()
        elif (self.isRunningRight):
            self.setMotorRight()
        else:
            self.stop()

    def setMotorLeftRightStop(self):
        self.isRunningLeft = 0
        self.isRunningRight = 0
        if (self.isRunningForward):
            self.setMotorForward()
        elif (self.isRunningReverse):
            self.setMotorReverse()
        else:
            self.stop()

    def setMotorLeft(self):
        self.isRunningLeft = 1
        self.isRunningRight = 0
        self.setMotors(self.motorTurnSpeed, self.motorTurnSpeed,
                       Adafruit_MotorHAT.BACKWARD, Adafruit_MotorHAT.FORWARD)

    def setMotorRight(self):
        self.isRunningLeft = 0
        self.isRunningRight = 1
        self.setMotors(self.motorTurnSpeed, self.motorTurnSpeed,
                       Adafruit_MotorHAT.FORWARD, Adafruit_MotorHAT.BACKWARD)
Beispiel #60
0
from Adafruit_PWM_Servo_Driver import PWM
import time

pwm = PWM(0x70)
pwm.setPWMFreq(50)

while (True):
    pwm.setPWM(0, 0, 120)
    time.sleep(0.3)
    pwm.setPWM(0, 0, 290)
    time.sleep(0.3)
    pwm.setPWM(0, 0, 480)
    time.sleep(0.3)