Example #1
0
 def __init__(self):
     # 四驱
     self.PWMA = 18
     self.AIN1 = 27
     self.AIN2 = 22
     self.PWMB = 23
     self.BIN1 = 25
     self.BIN2 = 24
     # 超声波模块
     self.Gpin = 5
     self.Rpin = 6
     self.TRIG = 20
     self.ECHO = 21
     # 红外线模块
     self.SensorRight = 16
     self.SensorLeft = 12
     self.T_SensorLeft = 13
     self.T_SensorRight = 26
     # Initialise the PWM device using the default address
     # bmp = PWM(0x40, debug=True)
     self.pwm = PWM(0x40, debug=False)
     # Min pulse length out of 4096
     self.servoMin = 150
     # Max pulse length out of 4096
     self.servoMax = 600
     # PWM系数
     self.DC2VC = 0.65
     self.L_Motor = None
     self.R_Motor = None
     # 初始化
     self.setup()
Example #2
0
    def init(self):
        print "Initializing Balancing Robot Code"
        """ Calibrate the ADXL345 Accelerometer """

        self.pwm = PWM(0x40, debug=True)
        self.pwm.setPWMFreq(200)

        GPIO.setmode(GPIO.BCM)
        GPIO.setup(18, GPIO.OUT)
        GPIO.setup(23, GPIO.OUT)
        GPIO.setup(24, GPIO.OUT)
        GPIO.setup(25, GPIO.OUT)

        self.pwm.setPWM(0, 0, 0)
        self.pwm.setPWM(1, 0, 0)

        proceed = raw_input("Is the robot level? (yes|no):")
        if not (proceed == "yes" or proceed == "Yes"):
            return

        calibrated_sum = 0.0
        for i in range(0, 100):
            print "Calibration step: %d (avg=%0.2f)" % (i, calibrated_sum /
                                                        (i + 1.0))
            calibrated_sum = calibrated_sum + globals.ACCEL
            time.sleep(0.05)
        self.calibrated_accel = calibrated_sum / 100.0

        # calibration complete
        print "Calibrated accelerometer value is %0.2f" % (
            self.calibrated_accel)
Example #3
0
def initialize():
    # Initialise the PWM device using the default address
    pwm = PWM(0x40)
    pwm.setPWMFreq(60)  # Set frequency to 60 Hz

    print "Hello, I'm Ruby the Rubik's Cube Robot!"
    print '\n'

    F_Open()
    B_Open()
    L_Open()
    R_Open()
    sleep(2)

    F_Close()
    B_Close()
    sleep(0.5)
    F_Default()
    B_Default()
    sleep(1)

    L_Close()
    R_Close()
    sleep(0.5)
    L_Default()
    R_Default()
    sleep(1)

    insert_cube()
Example #4
0
    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)
Example #5
0
 def __init__(self):
     print "qqqqqqqqqq"
     self.node_name = rospy.get_name()
     self.pwm = PWM(address=0x40, debug=False)
     self.pwm.setPWMFreq(60)
     self.pwm.setPWM(3, 0, 650)
     self.sub_joy_ = rospy.Subscriber("joy", Joy, self.cbJoy, queue_size=1)
Example #6
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)  
Example #7
0
class servo_control():

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

  def moving(self, pos):
    if pos.data < 200:
      #rospy.logwarn("Servo min/max values exceeded: %d.  Acceptable range: 140-710" % pos.data)
      pos.data = 200
    elif pos.data > 550:
      #rospy.logwarn("Servo min/max values exceeded: %d.  Acceptable range: 140-710" % pos.data)
      pos.data = 550

    self.pwm.setPWM(0, 0, pos.data)    

  def center():
    pwm = PWM(0x40)
    pwm.setPWMFreq(60)
    pwm.setPWM(0, 0, 345)

  def __init__(self):
    rospy.Subscriber("servo_position", Int16, self.moving)
    self.pwm = PWM(0x40)
    self.pwm.setPWMFreq(60)                        # Set frequency to 60 Hz

  rospy.on_shutdown(center)
Example #8
0
 def connect(self, address):
     if self.debug:
         print(
             "[connect:ServoDriver] connecting to the servo driver at i2c address ",
             address)
     self.pwm = PWM(address, debug=self.debug)
     self.pwm.setPWMFreq(self.freq)
Example #9
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))
Example #10
0
class PanTiltDevice:
    """A class for controlling the PanTilt Device"""
    pwm = PWM(0x40)
    PAN_SERVO_CHANNEL = 0
    TILT_SERVO_CHANNEL = 1

    def __init__(self):
        self.pwm = PWM(0x40)
        self.pwm.setPWMFreq(10)  # Set frequency to 60 Hz
        # self.panServo = Servo("pan", self.pwm, self.PAN_SERVO_CHANNEL, 280, 660, 0.2, 0.003)
        self.panServo = Servo("pan", self.pwm, self.PAN_SERVO_CHANNEL, 120, 700, 0.3, 0.01)
        self.tiltServo = Servo("tilt", self.pwm, self.TILT_SERVO_CHANNEL, 320, 650, 0.3, 0.02)
        self.panTarget = 50
        self.tiltTarget = 50
        self.init()

    def init(self):
        self.panAndTilt(self.panTarget, self.tiltTarget)

    def pan(self, percent):
        self.panTarget = percent
        # print "panning to %d percent" % self.panTarget
        self.panServo.setTargetPosition(self.panTarget)

    def tilt(self, percent):
        # print "tilting to %d percent" % percent
        self.tiltServo.setTargetPosition(percent)

    def panAndTilt(self, panPercent, tiltPercent):
        self.pan(panPercent)
        self.tilt(tiltPercent)
Example #11
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;
Example #12
0
class PanTiltDevice:
    """A class for controlling the PanTilt Device"""
    pwm = PWM(0x40)
    PAN_SERVO_CHANNEL = 0
    TILT_SERVO_CHANNEL = 1

    def __init__(self):
        self.pwm = PWM(0x40)
        self.pwm.setPWMFreq(10)  # Set frequency to 60 Hz
        # self.panServo = Servo("pan", self.pwm, self.PAN_SERVO_CHANNEL, 280, 660, 0.2, 0.003)
        self.panServo = Servo("pan", self.pwm, self.PAN_SERVO_CHANNEL, 150,
                              700, 0.002, 0.00005)
        self.tiltServo = Servo("tilt", self.pwm, self.TILT_SERVO_CHANNEL, 500,
                               680, 0.002, 0.00005)

    def pan(self, percent):
        # print "panning to %d percent" % self.panTarget
        self.panServo.setTargetPosition(percent)

    def tilt(self, percent):
        # print "tilting to %d percent" % percent
        self.tiltServo.setTargetPosition(percent)

    def panAndTilt(self, panPercent, tiltPercent):
        self.pan(panPercent)
        self.tilt(tiltPercent)
Example #13
0
	def __init__(self, f, name="LED"):
		Thread.__init__(self, name=name)
		self.RED, self.GREEN, self.BLUE = 15,1,14
		self.pw2 = PWM(0x60)
		#self.freq = f
		#self.pw2.setPWMFreq(self.freq)
		self.previous = led_attribute.state
Example #14
0
	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)
Example #15
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)
Example #16
0
class Servo:
    def __init__(self, channel, min, max, freq):
        self.pwm = PWM(0x40)
        self.pwm.setPWMFreq(freq)

        self.channel = channel
        self.min = min
        self.max = max
        self.range = max - min
        # get middle of the range
        self.current = 0
        self.move_to(0.5)

    def move_to(self, end_pos):
        p = Process(target=move_to_process,
                    args=(self.pwm,
                          self.current,
                          self.channel,
                          self.range,
                          self.min,
                          self.max,
                          end_pos))
        p.start()
        self.current = end_pos

    def get_dc_by_range(self, position):
        return self.range * position + self.min
Example #17
0
def init():
    global p, q, a, b, pwm, pcfADC, PGType
    PGType = PGFull
    # Initialise the PCA9685 PWM device using the default address
    try:
        pwm = PWM(0x40, debug = False)
        pwm.setPWMFreq(60)  # Set frequency to 60 Hz
    except:
        PGType = PGLite # No PCA9685 so set to Pi2Go-Lite

    #use physical pin numbering
    GPIO.setmode(GPIO.BOARD)

    #set up digital line detectors as inputs
    GPIO.setup(lineRight, GPIO.IN) # Right line sensor
    GPIO.setup(lineLeft, GPIO.IN) # Left line sensor

    #Set up IR obstacle sensors as inputs
    GPIO.setup(irFL, GPIO.IN) # Left obstacle sensor
    GPIO.setup(irFR, GPIO.IN) # Right obstacle sensor
    GPIO.setup(irMID, GPIO.IN) # Centre Front obstacle sensor

    #use pwm on inputs so motors don't go too fast
    GPIO.setup(L1, GPIO.OUT)
    p = GPIO.PWM(L1, 20)
    p.start(0)

    GPIO.setup(L2, GPIO.OUT)
    q = GPIO.PWM(L2, 20)
    q.start(0)

    GPIO.setup(R1, GPIO.OUT)
    a = GPIO.PWM(R1, 20)
    a.start(0)

    GPIO.setup(R2, GPIO.OUT)
    b = GPIO.PWM(R2, 20)
    b.start(0)

    # Initalise the ADC
    pcfADC = None # ADC object
    try:
        pcfADC = sgh_PCF8591P(1) #i2c, 0x48)
    except:
        PGType = PGLite

    # initialise servos (Pi2Go-Lite only)
    if PGType == PGLite:
        startServos()

    #set up Pi2Go-Lite White LEDs as outputs
        GPIO.setup(frontLED, GPIO.OUT)
        GPIO.setup(rearLED, GPIO.OUT)

    #set switch as input with pullup
    if PGType == PGLite:
        GPIO.setup(Lswitch, GPIO.IN, pull_up_down=GPIO.PUD_UP)
    else:
        GPIO.setup(switch, GPIO.IN, pull_up_down=GPIO.PUD_UP)
 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)
Example #19
0
def initialise_led():
  global pwm
  from Adafruit_PWM_Servo_Driver import PWM

  # Set LED parameters
  pwm = PWM(0x40)
  pwm.setPWMFreq(1000)
  print("LEDs are initialised")
Example #20
0
 def __init__(self):
     self.pwm = PWM(0x40)
     self.pwm.setPWMFreq(10)  # Set frequency to 60 Hz
     # self.panServo = Servo("pan", self.pwm, self.PAN_SERVO_CHANNEL, 280, 660, 0.2, 0.003)
     self.panServo = Servo("pan", self.pwm, self.PAN_SERVO_CHANNEL, 150,
                           700, 0.002, 0.00005)
     self.tiltServo = Servo("tilt", self.pwm, self.TILT_SERVO_CHANNEL, 500,
                            680, 0.002, 0.00005)
Example #21
0
 def __init__(self, debug=False):
     self.pwm = PWM(address=0x40, debug=debug)
     for i in range(40):
         self.pwm.setPWM(i, 0, 4095)
     self.pwm.setPWM(0, 4095, 4095)
     sleep(100)
     self.pwm.setPWM(0, 4095, 4095)
     print "-----"
Example #22
0
def initialise_led():
    global pwm
    from Adafruit_PWM_Servo_Driver import PWM

    # Set LED parameters
    pwm = PWM(0x40)
    pwm.setPWMFreq(1000)
    print("LEDs are initialised")
Example #23
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
Example #24
0
 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);
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)
 def __init__(self, addr=0x60, freq=1600, i2c=None, i2c_bus=None):
     self._frequency = 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, i2c=i2c, i2c_bus=i2c_bus)
     self._pwm.setPWMFreq(self._frequency)
 def begin(self, block = 0, address = 0x40):
     """Call begin() before any other meArm calls.  Optional parameters to select a different block of servo connectors or different I2C address."""
     self.pwm = PWM(address) # Address of Adafruit PWM servo driver
 	self.base = block * 4
 	self.shoulder = block * 4 + 1
 	self.elbow = block * 4 + 2
 	self.gripper = block * 4 + 3
 	self.pwm.setPWMFreq(60)
 	self.openGripper()
 	self.goDirectlyTo(0, 100, 50)
Example #28
0
 def begin(self, pwmFrequency = 60, block = 0, address = 0x40, busnum =1):
     """Call begin() before any other meArm calls.  Optional parameters to select a different block of servo connectors or different I2C address."""
     self.pwm = PWM(address, busnum) # Address of Adafruit PWM servo driver
 	self.base = block * 4
 	self.shoulder = block * 4 + 1
 	self.elbow = block * 4 + 2
 	self.gripper = block * 4 + 3
 	self.pwm.setPWMFreq(pwmFrequency)
 	self.closeGripper()
     self.home()
Example #29
0
 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)
Example #30
0
 def begin(self, block = 0, address = 0x40, busnum =1):
     """Call begin() before any other meArm calls.  Optional parameters to select a different block of servo connectors or different I2C address."""
     self.pwm = PWM(address, busnum) # Address of Adafruit PWM servo driver
 	self.base = block * 4
 	self.shoulder = block * 4 + 1
 	self.elbow = block * 4 + 2
 	self.gripper = block * 4 + 3
 	self.pwm.setPWMFreq(PWM_FREQUENCY)
 	self.closeGripper()
 	self.goDirectlyTo(HOME_X, HOME_Y, HOME_Z)
Example #31
0
 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
Example #32
0
def init():
    #-- initialization
    pwm = PWM(0x40)
    pwm.setPWMFreq(60)  # Set frequency to 60 Hz
    setServoPulse(pwm, 1, 100)  #
    print "delta=%d, oneDeg=%f" % (delta, oneDeg)
    for i in channels:
        curAngles[i] = 0
    ready(pwm)
    return pwm
Example #33
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
Example #34
0
    def __init__(self, proto, serverIP, serverPORT):
        # Init class
        self.proto = proto
        self.serverIP = serverIP
        self.serverPORT = serverPORT
        self.serverAddr = self.proto + "://" + self.serverIP + ":" + self.serverPORT
        self.connect()

        # Init PWM Driver
        self.pwm = PWM(0x40, debug=config.get('control', 'pwm_debug'))
        self.pwm.setPWMFreq(config.get('control', 'pwm_freq'))
Example #35
0
    def __init__(self):
        self.ADDRESSE = 0x40
        self.MIN_PULSE_LENGTH = 150
        self.MAX_PULSE_LENGTH = 600
        self.FREQUENCY = 60
        self.CLAMP_VALUES = [[self.MIN_PULSE_LENGTH, self.MAX_PULSE_LENGTH]
                             ] * 15
        self.pwm = PWM(self.ADDRESSE)
        self.INDEX = 0

        print(self.CLAMP_VALUES)
Example #36
0
    def __init__(self, channel, min, max, freq):
        self.pwm = PWM(0x40)
        self.pwm.setPWMFreq(freq)

        self.channel = channel
        self.min = min
        self.max = max
        self.range = max - min
        # get middle of the range
        self.current = 0
        self.move_to(0.5)
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
 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
Example #39
0
 def __init__(self, name):
     self.name = name
     self.type = 'pca9685_pwm'
     self.channel_config = []
     self.i2c_address = None
     self.dmx_channel_start = None
     self.dmx_channel_end = None
     self.handler = self.default_handler
     self.servo_min = 103  # Min pulse length out of 4096 = 20mS
     self.servo_max = 500  # Max pulse length out of 4096
     self.pwm = PWM(0x40, debug=True)
     self.pwm.setPWMFreq(50)  # Set frequency to 60 Hz
Example #40
0
 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)
     ]
Example #41
0
    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)]
Example #42
0
class ControlServer(object):

    payload = {}
    cycles = 0
    debug = False
    telem = False

    def __init__(self, proto, serverIP, serverPORT):
        # Init class
        self.proto = proto
        self.serverIP = serverIP
        self.serverPORT = serverPORT
        self.serverAddr = self.proto+"://"+self.serverIP+":"+self.serverPORT
        self.connect()

        # Init PWM Driver
        self.pwm = PWM(0x40, debug=config.get('control', 'pwm_debug'))
        self.pwm.setPWMFreq(config.get('control', 'pwm_freq'))

    def connect(self):
        # Bind socket for listening
        logging.info("Binding to address: %s", self.serverAddr)
        self.context = zmq.Context(1)
        self.socket  = self.context.socket(zmq.REP)
        self.socket.bind(self.serverAddr)

    def process(self):
        log_every = config.get('control', 'logevery')

        # Receive the payload from server
        payload = None
        payload = json.loads(self.socket.recv())
        sequence = payload['seq']
        self.cycles += 1

        # Log Heartbeat
        if(sequence % log_every) == 0:
            logging.info("Heartbeat Alive. Sequence # %s", sequence)

        # Get Telemetry data
        # if(sequence % (log_every / 4)) == 0:
        #     self.telem = dk.update
        #     logging.info(self.telem)

        # Send ack sequence back to server
        self.socket.send(str(sequence))
        payload.pop('seq', None)
        if self.debug:
            logging.info(payload)
        return payload
Example #43
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
    def __init__(self, filter, frequency):
        self.filter = filter
        self.target_rotation = [0, 0]
        pwm = PWM(0x40)
        pwm.setPWMFreq(frequency)
        self.motors = [
            Motor("front_left", pwm, 4),
            Motor("front_right", pwm, 5),
            Motor("back_right", pwm, 6),
            Motor("back_left", pwm, 7)
        ]

        #let the motor'ss/ecu's init properly
        time.sleep(3)
Example #45
0
def init():
    global p, q, a, b, pwm, pcfADC, PGType
    PGType = PI2KF
    # Initialise the PCA9685 PWM device using the default address
    try:
        pwm = PWM(0x60, debug = False)
        pwm.setPWMFreq(60)
    except:
        print "can't set PWM frequency!"

    mh.getMotor(1).run(Adafruit_MotorHAT.RELEASE)
    mh.getMotor(2).run(Adafruit_MotorHAT.RELEASE)
    mh.getMotor(3).run(Adafruit_MotorHAT.RELEASE)
    mh.getMotor(4).run(Adafruit_MotorHAT.RELEASE)
Example #46
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
Example #47
0
	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)
Example #48
0
 def init_gpio(self):
     """!Initialize the GPIO pins"""
     if have_rpi_gpio:
         GPIO.setmode(GPIO.BOARD)
         if (self.config.StatusRGBPins.red):
             GPIO.setup(self.config.StatusRGBPins.red,GPIO.OUT, initial=GPIO.LOW)
             GPIO.setup(self.config.StatusRGBPins.green,GPIO.OUT, initial=GPIO.LOW)
             GPIO.setup(self.config.StatusRGBPins.blue,GPIO.OUT, initial=GPIO.LOW)
             self.set_rgb_status(1,1,1)
         if (self.config.pwm_i2c_address>0):
             self.pwm = PWM(self.config.pwm_i2c_address)
             self.pwm.setPWMFreq(self.config.pwm_frequency)
         if (self.config.Motor1.dir>0 and self.config.Motor1.hardware=="gpio"):
             GPIO.setup(self.config.Motor1.dir,GPIO.OUT, initial=GPIO.LOW)    
             GPIO.setup(self.config.Motor1.step,GPIO.OUT, initial=GPIO.LOW)    
             GPIO.setup(self.config.Motor1.sleep,GPIO.OUT, initial=GPIO.LOW)    
             if (self.config.Motor1.stopsensor>0):
                 GPIO.setup(self.config.Motor1.stopsensor,GPIO.IN, GPIO.PUD_UP)    
         if (self.config.Motor2.dir>0 and self.config.Motor2.hardware=="gpio"):
             GPIO.setup(self.config.Motor2.dir,GPIO.OUT, initial=GPIO.LOW)    
             GPIO.setup(self.config.Motor2.step,GPIO.OUT, initial=GPIO.LOW)    
             GPIO.setup(self.config.Motor2.sleep,GPIO.OUT, initial=GPIO.LOW)
             if (self.config.Motor2.stopsensor>0):
                 GPIO.setup(self.config.Motor2.stopsensor,GPIO.IN, GPIO.PUD_UP)
         if (self.config.Motor1.hardware=="motor_ctl"):
             self.init_motor_ctl()
Example #49
0
  def init(self):
    print "Initializing Balancing Robot Code"
  
    """ Calibrate the ADXL345 Accelerometer """

    self.pwm = PWM(0x40, debug=True)
    self.pwm.setPWMFreq(200)

    GPIO.setmode(GPIO.BCM)
    GPIO.setup(18, GPIO.OUT)
    GPIO.setup(23, GPIO.OUT)
    GPIO.setup(24, GPIO.OUT)
    GPIO.setup(25, GPIO.OUT)

    self.pwm.setPWM(0, 0, 0)
    self.pwm.setPWM(1, 0, 0)

    proceed = raw_input("Is the robot level? (yes|no):")
    if not (proceed == "yes" or proceed == "Yes"):
      return

    calibrated_sum = 0.0
    for i in range(0,100):
      print "Calibration step: %d (avg=%0.2f)" % (i, calibrated_sum / (i+1.0))
      calibrated_sum = calibrated_sum + globals.ACCEL
      time.sleep(0.05)
    self.calibrated_accel = calibrated_sum / 100.0

    # calibration complete
    print "Calibrated accelerometer value is %0.2f" % (self.calibrated_accel)
Example #50
0
	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()
Example #51
0
    def __init__(self, i2caddress, channel, pwm_min, pwm_max, reversed=False, offset=0, minangle=-90, maxangle=90,
                 callback=None):
        # TODO: memorize max and min positions and set functions reaching it
        self.i2caddress = i2caddress
        self._pwm = PWM(self.i2caddress, debug=False)  # Set debug to False before release

        self.channel = channel
        self._pwm.setPWMFreq(60)

        self.pwm_min = pwm_min  # pwm_min: HS-485HB->158 HS-645MG->149
        self.pwm_max = pwm_max  # pwm_max: HS-485HB->643 HS-645MG->651
        self._actualpwm = None

        self.angle = None
        self.anglethrow = 180  # Maximum angle throw of servo end-to-end
        self.offset = offset
        self.reversed = reversed
        self.callback = callback

        self.powered = None
        self.poweroff()

        if maxangle - minangle > self.anglethrow:
            sys.exit('Result of maxAngle-minAngle exceeds predefined servo throw of %s' % self.anglethrow)
        elif not (self.setminangle(minangle) and self.setmaxangle(maxangle)):
            sys.exit('Error initializing servo on channel #%s. Unable to set min/max values' % channel)
Example #52
0
	def __init__(self):
		self.pwm = PWM(0x40, debug=True)
		self.freq = 10
		self.pwm.setPWMFreq(self.freq)
		self.red_pin = 1
		self.green_pin = 2
		self.blue_pin = 3

		self.light_state = False
		self.reading_light = {'red': 255, 'green': 255, 'blue': 255}
		self.reading_light['red'] = self.reading_light['red'] * 16
		self.reading_light['green'] = self.reading_light['green'] * 16
		self.reading_light['blue'] = self.reading_light['blue'] * 16
		print 'red: ' + str(self.reading_light['red'])

		self.colour = {'red': 0, 'green': 0, 'blue': 0}

		self.pwm.setPWM(self.red_pin, 0 , 0)
		self.pwm.setPWM(self.green_pin, 0, 0)
		self.pwm.setPWM(self.blue_pin, 0, 0)

		self.fade_loop_stop = False
		end_time = datetime.datetime.now()
		self.fade_end_time = end_time
		black = {'red': 0, 'green': 0, 'blue': 0}
		self.fade_diffs_dict = self.fade_diffs(black, black)
		self.fade_total_duration = datetime.timedelta(seconds=0)

		self.fade_loop = Thread(target=self.fade2)
		self.fade_loop.start()
		print '{' * 20
		print self.fade_loop.is_alive()
		print '}' * 20
Example #53
0
	def __init__( self ):

		# Register an exit function
		atexit.register( self._exit_handler )

		# Initialize the gpio pins
		GPIO.setmode( GPIO.BCM )
		GPIO.setup( L_GPIO, GPIO.OUT )
		GPIO.setup( R_GPIO, GPIO.OUT )

		GPIO.setup( L_ENCODER, GPIO.IN, pull_up_down=GPIO.PUD_UP )
		GPIO.setup( R_ENCODER, GPIO.IN, pull_up_down=GPIO.PUD_UP )

		GPIO.add_event_detect( L_ENCODER, GPIO.FALLING, callback=self.encoder, bouncetime=5 )
		GPIO.add_event_detect( R_ENCODER, GPIO.FALLING, callback=self.encoder, bouncetime=5 )

		self.last  = { L_ENCODER: 0, R_ENCODER: 0 }
		self.count = { L_ENCODER: 0, R_ENCODER: 0 }

		# Initialize the pwm board
		self.pwm = PWM( PWM_ADDRESS )
		self.pwm.setPWMFreq( PWM_FREQUENCY )

		# Initialize variables
		self.speed = 0
Example #54
0
    def __init__(self, main_motor_hat, params=None):
        # Motor hat for locomotion
        self.main_motor_hat = main_motor_hat
        self.motors = {}
        self.motors["b_left"] = {"motor" : self.main_motor_hat.getMotor(1), "target" : 0.0, "scaler" : 1.0}
        self.motors["f_left"] = {"motor" : self.main_motor_hat.getMotor(2), "target" : 0.0, "scaler" : -1.0}
        self.motors["b_right"] = {"motor" : self.main_motor_hat.getMotor(3), "target" : 0.0, "scaler" : -1.0}
        self.motors["f_right"] = {"motor" : self.main_motor_hat.getMotor(4), "target" : 0.0, "scaler" : 1.0}

        self.params = params if params is not None else {}

        self.servo_controller = PWM(SERVO_HAT_I2C_ADDR)

        self.servo_pwm_freq_hz = self.params.get("servo_pwm_freq_hz", 48)
        self.servo_max_angle_deg = self.params.get("servo_max_angle_deg", 45.0)
        self.servo_max_angle_us = self.params.get("servo_max_angle_us", 400)
        self.servo_neutral_us = self.params.get("servo_neutral_us", 1520)
        # Correction factor to apply to each duration to match the
        # internal oscillator of the PCA9685 on the Servo HAT. The internal
        # RC clock is supposed to be 25MHz, but it can be off
        self.servo_clock_k = self.params.get("servo_clock_k", 1.073446)
        self.servo_controller.setPWMFreq(self.servo_pwm_freq_hz)

        # Queue for input events
        self.input_queue = Queue.Queue(1)
        self.thread = Thread(target=self.process)
Example #55
0
	def __init__(self):
		self.pwm = PWM(0x40, debug=True)
		self.servoMin = 400
		self.servoMax = 600
		self.servoReverse = 440
		self.vertical_pin = 0
		self.left_pin = 1
		self.right_pin = 2
Example #56
0
    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
Example #57
0
 def __init__(self):
     self.pwm = PWM(0x40)
     self.pwm.setPWMFreq(10)  # Set frequency to 60 Hz
     self.panServo = Servo("pan", self.pwm, self.PAN_SERVO_CHANNEL, 180, 630, 0.2, 0.003)
     self.tiltServo = Servo("tilt", self.pwm, self.TILT_SERVO_CHANNEL, 450, 600, 0.4, 0.005)
     self.panTarget = 50
     self.tiltTarget = 50
     self.init()
Example #58
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()