예제 #1
0
파일: ServoAPI.py 프로젝트: xyu6/robot_car
class ServoAPI():
    debug = False
    pwm = None

    servoMin = 150  # Min pulse length out of 4096
    servoMax = 550  # Max pulse length out of 4096
    servoMid = 350
    curServo = 0

    def __init__(self, debug=False):
        self.debug = debug
        self.pwm = PWM(0x6F, debug=False)

    def left(self):
        if (self.curServo != self.servoMax):
            self.pwm.setPWM(0, 0, self.servoMax)
            self.curServo = self.servoMax

    def right(self):
        if (self.curServo != self.servoMin):
            self.pwm.setPWM(0, 0, self.servoMin)
            self.curServo = self.servoMin

    def forward(self):
        if (self.curServo != self.servoMid):
            self.pwm.setPWM(0, 0, self.servoMid)
            self.curServo = self.servoMid

    def stop(self):
        self.pwm.setPWM(0, 0, 0)
        self.curServo = 0

    def init(self):
        self.pwm.setPWMFreq(60)
        self.curServo = 0
예제 #2
0
	def __init__(self, addr = 0x60, freq = 1600):
		self._i2caddr = addr            # default addr on HAT
		self._frequency = freq		# default @1600Hz PWM freq
		self.motors = [ Raspi_DCMotor(self, m) for m in range(4) ]
		self.steppers = [ Raspi_StepperMotor(self, 1), Raspi_StepperMotor(self, 2) ]
		self._pwm =  PWM(addr, debug=False)
		self._pwm.setPWMFreq(self._frequency)
예제 #3
0
 def __init__(self):
     # Controling with gpiozero source: https://gpiozero.readthedocs.io/en/stable/api_output.html
     self.move = {'forward': self.move_forward, 'backward': self.move_backward, 'right': self.turn_right, 'left': self.turn_left, 'stop turn': self.stop_turn, 'stop forward/backward': self.stop_forward_reverse}
     self.drive_motor = gpio.Motor(24,23)
     self.drive_motor_2 = gpio.Motor(16,20)
     self.pwm = PWM(0x6F)
     self.pwm.setPWMFreq(60)
     self.motor_angle = 370.0
예제 #4
0
파일: car.py 프로젝트: KimUJin3359/IoT_Car
    def run(self):
        db = QtSql.QSqlDatabase.addDatabase('QMYSQL')
        db.setHostName("13.209.64.111")
        db.setDatabaseName("IoT_data")
        db.setUserName("ujin")
        db.setPassword("ujin")
        ok = db.open()
        print(ok)

        self.mh = Raspi_MotorHAT(addr=0x6f)
        self.myMotor = self.mh.getMotor(2)
        self.myMotor.setSpeed(100)
        self.speed_value = 100

        self.pwm = PWM(0x6F)
        self.pwm.setPWMFreq(60)

        #self.sense=SenseHat()

        self.BUZZER = 5
        self.RIGHT_LED_F = 23
        self.LEFT_LED_F = 24
        self.RIGHT_LED_R = 8
        self.LEFT_LED_R = 7
        self.ECHO = 21
        self.TRIG = 20

        self.timeF = 0
        self.camera = picamera.PiCamera()

        GPIO.setmode(GPIO.BCM)
        GPIO.setwarnings(False)
        GPIO.setup(self.BUZZER, GPIO.OUT)
        GPIO.setup(self.RIGHT_LED_F, GPIO.OUT)
        GPIO.setup(self.LEFT_LED_F, GPIO.OUT)
        GPIO.setup(self.RIGHT_LED_R, GPIO.OUT)
        GPIO.setup(self.LEFT_LED_R, GPIO.OUT)
        GPIO.setup(self.TRIG, GPIO.OUT)
        GPIO.setup(self.ECHO, GPIO.IN)

        while True:
            if (self.timeF == 10):
                now = datetime.datetime.now()
                self.camera.capture('./img/' + now.strftime('%Y%m%d%H%M%S') +
                                    '.jpg')
                self.timeF = 0
            time.sleep(0.1)
            self.getQuery()
            # self.setQuery()
            self.timeF += 1
예제 #5
0
 def __init__(self, addr=0x60, freq=1600):
     self._i2caddr = addr  # default addr on HAT
     self._frequency = freq  # default @1600Hz PWM freq
     self.motors = [Raspi_DCMotor(self, m) for m in range(4)]
     self.steppers = [Raspi_StepperMotor(self, 1), Raspi_StepperMotor(self, 2)]
     self._pwm = PWM(addr, debug=False)
     self._pwm.setPWMFreq(self._frequency)
예제 #6
0
class ServoAPI():
    pwm = PWM(0x6F, debug=True)

    servoMin = 150  # Min pulse length out of 4096
    servoMax = 600  # Max pulse length out of 4096
    servoMid = 350
    curServo = 0

    def left(self):
        if (self.curServo != self.servoMax):
            self.pwm.setPWM(0, 0, self.servoMax)
            self.curServo = self.servoMax

    def right(self):
        if (self.curServo != self.servoMin):
            self.pwm.setPWM(0, 0, self.servoMin)
            self.curServo = self.servoMin

    def forward(self):
        if (self.curServo != self.servoMid):
            self.pwm.setPWM(0, 0, self.servoMid)
            self.curServo = self.servoMid

    def stop(self):
        self.curServo = 0

    def init(self):
        self.pwm.setPWMFreq(60)
        self.curServo = 0
예제 #7
0
 def __init__(self, default_h_pos = START_HORIZONTAL_POS, default_v_pos = START_VERTICAL_POS):
     self.pwm = PWM(0x6F, debug=True)
     self.pwm.setPWMFreq(50)
     self.current_horizontal_pos = default_h_pos
     self.current_vertical_pos = default_v_pos
     self.pwm.setPWM(HORIZONTAL_CHANNEL, 0, int(self.current_horizontal_pos))
     self.pwm.setPWM(VERTICAL_CHANNEL, 0, int(self.current_vertical_pos))
예제 #8
0
class Raspi_MotorHAT:
    FORWARD = 1
    BACKWARD = 2
    BRAKE = 3
    RELEASE = 4

    SINGLE = 1
    DOUBLE = 2
    INTERLEAVE = 3
    MICROSTEP = 4

    def __init__(self, addr=0x60, freq=50):
        self._i2caddr = addr  # default addr on HAT
        self._frequency = freq  # default @1600Hz PWM freq
        self.motors = [Raspi_DCMotor(self, m) for m in range(4)]
        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 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]
예제 #9
0
 def run(self):    
   self.db = QtSql.QSqlDatabase.addDatabase('QMYSQL')
   self.db.setHostName("3.34.124.67")
   self.db.setDatabaseName("16_3")
   self.db.setUserName("16_3")
   self.db.setPassword("1234")
   ok = self.db.open()
   print(ok)
   if ok == False : return  
   
   self.mh = Raspi_MotorHAT(addr=0x6f)
   self.myMotor = self.mh.getMotor(1)
         
   self.pwm = PWM(0x6F)
   self.pwm.setPWMFreq(60)
   
   self.sense = SenseHat(1)
   
   while True:
     time.sleep(0.1)
     self.getQuery()
     self.setQuery()
예제 #10
0
def updateServoMotorPositions( picth , yaw ):
    # Initialise the PWM device using the default address
    # bmp = PWM(0x40, debug=True)
    pwm = PWM(0x6F)
    pwm.setPWMFreq(60)  # Set frequency to 60 Hz
    servoMin1 = 150  # Min pulse length out of 4096
    #servoMid = 375  # Min pulse length out of 4096
    servoMax1 = 490  # Max pulse length out of 4096
    servoMin0 = 250  # Min pulse length out of 4096
    #servoMid = 375  # Min pulse length out of 4096
    servoMax0 = 600  # Max pulse length out of 4096
    pulsePerAngle1 = 2.5
    pulsePerAngle0 = 2.5
    pulsePicth = int(250 + (picth-20) *pulsePerAngle0 )  #caculating angle to pulse
    pulseYaw = int(150 + (yaw -2)* pulsePerAngle1)

    pwm.setPWM(0, 0, pulsePicth)   #output to servo
    pwm.setPWM(1, 0, pulseYaw)
예제 #11
0
class MotorControl:
# Source for pwm servo control http://raspberrypiwiki.com/index.php/File:Raspi-MotorHAT-python3.zip
    def __init__(self):
        # Controling with gpiozero source: https://gpiozero.readthedocs.io/en/stable/api_output.html
        self.move = {'forward': self.move_forward, 'backward': self.move_backward, 'right': self.turn_right, 'left': self.turn_left, 'stop turn': self.stop_turn, 'stop forward/backward': self.stop_forward_reverse}
        self.drive_motor = gpio.Motor(24,23)
        self.drive_motor_2 = gpio.Motor(16,20)
        self.pwm = PWM(0x6F)
        self.pwm.setPWMFreq(60)
        self.motor_angle = 370.0
        # self.turn_motor = gpio.AngularServo(0,45,-45)
    def function(self, instruction):
        # Running methods from a dictionary souce: https://stackoverflow.com/questions/36849108/calling-a-function-from-within-a-dictionary, user: alecxe
       self.move[instruction]()

    def move_forward(self):
        self.drive_motor.forward(1) # 1 moves forward, 0 stops motor
        self.drive_motor_2.forward(1) # 1 moves forward, 0 stops motor

        print("moving forward")

    def move_backward(self):
        self.drive_motor.backward(1) # 1 Moves backward, 0 stops motor
        self.drive_motor_2.backward(1) # 1 moves forward, 0 stops motor

        print("moving backward")

    def turn_right(self):# Turns vehicle right
        if self.motor_angle < 404: # Max angle
            self.motor_angle += 0.15
        self.pwm.setPWM(0, 0, int(self.motor_angle))
        # time.sleep(1)
        print("turn right")


    def turn_left(self): # Turns vehicle left
        # if self.turn_motor.angle > -45: # Max angle
            # self.turn_motor.angle -= 1
        if self.motor_angle > 340: # Max angle
            self.motor_angle -= 0.15
        self.pwm.setPWM(0, 0, int(self.motor_angle))

        print("turn left")

    def stop_turn(self):
        print('stop turn')

    def stop_forward_reverse(self): # Stops forward / backward movement
        self.drive_motor.forward(0)
        self.drive_motor.backward(0)
        self.drive_motor_2.forward(0)
        self.drive_motor_2.backward(0)
        print('stop forward back')
예제 #12
0
class Raspi_MotorHAT:
    FORWARD = 1
    BACKWARD = 2
    BRAKE = 3
    RELEASE = 4

    SINGLE = 1
    DOUBLE = 2
    INTERLEAVE = 3
    MICROSTEP = 4

    def __init__(self, addr=0x60, freq=1600):
        self._i2caddr = addr  # default addr on HAT
        self._frequency = freq  # default @1600Hz PWM freq
        self.motors = [Raspi_DCMotor(self, m) for m in range(4)]
        self.steppers = [Raspi_StepperMotor(self, 1), Raspi_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]
예제 #13
0
class pollingThread(QThread):
  def __init__(self):
    super().__init__()
  
  def run(self):    
    self.db = QtSql.QSqlDatabase.addDatabase('QMYSQL')
    self.db.setHostName("3.34.124.67")
    self.db.setDatabaseName("16_3")
    self.db.setUserName("16_3")
    self.db.setPassword("1234")
    ok = self.db.open()
    print(ok)
    if ok == False : return  
    
    self.mh = Raspi_MotorHAT(addr=0x6f)
    self.myMotor = self.mh.getMotor(1)
          
    self.pwm = PWM(0x6F)
    self.pwm.setPWMFreq(60)
    
    self.sense = SenseHat(1)
    
    while True:
      time.sleep(0.1)
      self.getQuery()
      self.setQuery()
  
  def setQuery(self):
    pressure = self.sense.get_pressure()
    temp = self.sense.get_temperature()
    humidity = self.sense.get_humidity()

    p = round((pressure - 1000) / 100, 3)
    t = round(temp / 100, 3)
    h = round(humidity / 100, 3)

    self.query = QtSql.QSqlQuery();
    self.query.prepare("insert into sensing2 (time, num1, num2, num3, meta_string, is_finish) values (:time, :num1, :num2, :num3, :meta, :finish)");
    time = QDateTime().currentDateTime()
    self.query.bindValue(":time", time)
    self.query.bindValue(":num1", p)
    self.query.bindValue(":num2", t)
    self.query.bindValue(":num3", h)
    self.query.bindValue(":meta", "")
    self.query.bindValue(":finish", 0)
    self.query.exec()
    
    a = int((p * 1271) % 256)
    b = int((t * 1271) % 256)
    c = int((h * 1271) % 256)
    self.sense.clear(a, b, c)

  def getQuery(self):
    query = QtSql.QSqlQuery("select * from command2 order by time desc limit 1");
    query.next()
    cmdTime = query.record().value(0)
    cmdType = query.record().value(1)
    cmdArg = query.record().value(2)
    is_finish = query.record().value(3)
          
    if is_finish == 0 :
      print(cmdTime.toString(), cmdType, cmdArg)

      query = QtSql.QSqlQuery("update command2 set is_finish=1 where is_finish=0");

      if cmdType == "go": self.go()        
      if cmdType == "back": self.back()
      if cmdType == "left": self.left()
      if cmdType == "right": self.right()
      if cmdType == "mid": self.middle()
      
      #add
      if cmdType == "front" and cmdArg == "press" : 
        self.go()
        self.middle()
      if cmdType == "leftside" and cmdArg == "press" : 
        self.go()
        self.left()
      if cmdType == "rightside" and cmdArg == "press" : 
        self.go()
        self.right()
        
      if cmdType == "front" and cmdArg == "release" : self.stop()
      if cmdType == "leftside" and cmdArg == "release" : self.stop()
      if cmdType == "rightside" and cmdArg == "release" : self.stop()
              
  def stop(self):
    print("MOTOR STOP")
    self.myMotor.run(Raspi_MotorHAT.RELEASE)

  def go(self):
    print("MOTOR GO")
    self.myMotor.setSpeed(50)
    self.myMotor.run(Raspi_MotorHAT.FORWARD)
    
  def back(self):
    print("MOTOR BACK")
    self.myMotor.setSpeed(50)
    self.myMotor.run(Raspi_MotorHAT.BACKWARD)

  def left(self):
    print("MOTOR LEFT")
    self.pwm.setPWM(0, 0, 150);

  def right(self):
    print("MOTOR RIGHT")
    self.pwm.setPWM(0, 0, 430);

  def middle(self):
    print("MOTOR MIDDLE")
    self.pwm.setPWM(0, 0, 350);
예제 #14
0
#!/usr/bin/python

from Raspi_PWM_Servo_Driver import PWM
import time

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

# Initialise the PWM device using the default address
# bmp = PWM(0x40, debug=True)
pwm = PWM(0x6F)

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

# Declare and initailize the counter
#!/usr/bin/python

from Raspi_PWM_Servo_Driver import PWM
import time

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

# Initialise the PWM device using the default address
# bmp = PWM(0x40, debug=True)
pwm = PWM(0x6F, 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)
예제 #16
0
#!/usr/bin/python

from Raspi_PWM_Servo_Driver import PWM
import time

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

# Initialise the PWM device using the default address
# bmp = PWM(0x40, debug=True)
pwm = PWM(0x6F)

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
for repeat in range(3):
    # Change speed of continuous servo on channel O
예제 #17
0
from Raspi_PWM_Servo_Driver import PWM
import time

# Initialise the PWM device using the default address
pwm = PWM(0x6F)

# Configure min and max servo pulse lengths
SERVOMIN = 150  # Min pulse length, us (tick 184/4096)
SERVOMAX = 550  # Max pulse length, us  (tick 430/4096)
servoMid = SERVOMAX - ((SERVOMAX - SERVOMIN) / 2)  # Midpoint pulse length, us
FREQUENCY = 50  # frequency length, Hz

pwm.setPWMFreq(FREQUENCY)  # Set frequency


def map(x, in_min, in_max, out_min, out_max):
    return int((x - in_min) * (out_max - out_min) / (in_max - in_min) +
               out_min)


def pulseWidthCal(inputAngle):
    pulse_wide = map(inputAngle, 0, 180, SERVOMIN, SERVOMAX)
    print("Current pulse is:", pulse_wide)
    return pulse_wide


print('Moving servo on channel 0, press Ctrl-C to quit...')
while (True):
    print("Going to servo 0 Degree...")
    pwm.setPWM(0, 0, pulseWidthCal(0))
    time.sleep(5)
예제 #18
0
# Import needed modules
from Raspi_PWM_Servo_Driver import PWM
import time

# Initialise the PWM device using the default address
# bmp = PWM(0x40, debug=True)
pwm = PWM(0x6F)

# Define constants
SERVO_MIN = 275  # Min pulse length out of 4096
SERVO_MAX = 475  # Max pulse length out of 4096
SERVO_MID = 375


#####################################################################
# Function:     step_0
# Input:        None
# Output:       None - void
# Description:	Move the servo to the far right position
#####################################################################
def step_0():
    # Set frequency to 60 Hz
    pwm.setPWMFreq(60)

    # Set the pulse width modulation
    pwm.setPWM(0, 0, SERVO_MIN)


#####################################################################
# Function:     step_1
# Input:        None
예제 #19
0
#!/usr/bin/python

from Raspi_PWM_Servo_Driver import PWM
import time

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

# Initialise the PWM device using the default address
# bmp = PWM(0x40, debug=True)
pwm = PWM(0x6F)

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, 375)
  pwm.setPWM(1, 0, 375)
예제 #20
0
Attributes:
    pwm: Initialize PWM device.
    yServoCenter: Centered position of servo along y-axis.
    xServoCenter: Centered position of servo along x-axis.
    yLim: Limit of displacement from center along y-axis.
    xMax: Maximum pwm position of servo along x-axis.
    xMin: Minimum pwm position of servo along x-axis.
    yMax: Maximum pwm position of servo along y-axis.
    yMin: Minimum pwm position of servo along y-axis."""

from Raspi_PWM_Servo_Driver import PWM
import numpy as np

# Initialise the PWM device using the default address
# bmp = PWM(0x40, debug=True)
pwm = PWM(0x6F, debug=True)

yServoCenter = 400
xServoCenter = 400
yLim = 210
xMax = 600
xMin = 200
yMax = 590
yMin = 270


def setServoPulse(channel, pulse):
    """Set pulse width modulation of servo."""
    pulseLength = 1000000  # 1,000,000 us per second
    pulseLength /= 60  # 60 Hz
    ##  print ("%d us per period" % pulseLength)
예제 #21
0
파일: ServoAPI.py 프로젝트: xyu6/robot_car
 def __init__(self, debug=False):
     self.debug = debug
     self.pwm = PWM(0x6F, debug=False)
예제 #22
0
class Raspi_MotorHAT:
    FORWARD = 1
    BACKWARD = 2
    BRAKE = 3
    RELEASE = 4

    SINGLE = 1
    DOUBLE = 2
    INTERLEAVE = 3
    MICROSTEP = 4

    def __init__(self, addr=0x60, freq=1600):
        self._i2caddr = addr  # default addr on HAT
        self._frequency = freq  # default @1600Hz PWM freq
        self.motors = [Raspi_DCMotor(self, m) for m in range(4)]
        self.steppers = [
            Raspi_StepperMotor(self, 1),
            Raspi_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]
        MICROSTEPS = 8
        MICROSTEP_CURVE = [0, 50, 98, 142, 180, 212, 236, 250, 255]

        #MICROSTEPS = 16
        # a sinusoidal curve NOT LINEAR!
        #MICROSTEP_CURVE = [0, 25, 50, 74, 98, 120, 141, 162, 180, 197, 212, 225, 236, 244, 250, 253, 255]

        def __init__(self, controller, num, steps=200):
            self.MC = controller
            self.revsteps = steps
            self.motornum = num
            self.sec_per_step = 0.1
            self.steppingcounter = 0
            self.currentstep = 0

            num -= 1

            if (num == 0):
                self.PWMA = 8
                self.AIN2 = 9
                self.AIN1 = 10
                self.PWMB = 13
                self.BIN2 = 12
                self.BIN1 = 11
            elif (num == 1):
                self.PWMA = 2
                self.AIN2 = 3
                self.AIN1 = 4
                self.PWMB = 7
                self.BIN2 = 6
                self.BIN1 = 5
            else:
                raise NameError(
                    'MotorHAT Stepper must be between 1 and 2 inclusive')

        def setSpeed(self, rpm):
            self.sec_per_step = 60.0 / (self.revsteps * rpm)
            self.steppingcounter = 0

        def oneStep(self, dir, style):
            pwm_a = pwm_b = 255

            # first determine what sort of stepping procedure we're up to
            if (style == Raspi_MotorHAT.SINGLE):
                if ((self.currentstep / (self.MICROSTEPS / 2)) % 2):
                    # we're at an odd step, weird
                    if (dir == Raspi_MotorHAT.FORWARD):
                        self.currentstep += self.MICROSTEPS / 2
                    else:
                        self.currentstep -= self.MICROSTEPS / 2
            else:
                # go to next even step
                if (dir == Raspi_MotorHAT.FORWARD):
                    self.currentstep += self.MICROSTEPS
                else:
                    self.currentstep -= self.MICROSTEPS
            if (style == Raspi_MotorHAT.DOUBLE):
                if not (self.currentstep / (self.MICROSTEPS / 2) % 2):
                    # we're at an even step, weird
                    if (dir == Raspi_MotorHAT.FORWARD):
                        self.currentstep += self.MICROSTEPS / 2
                    else:
                        self.currentstep -= self.MICROSTEPS / 2
                else:
                    # go to next odd step
                    if (dir == Raspi_MotorHAT.FORWARD):
                        self.currentstep += self.MICROSTEPS
                    else:
                        self.currentstep -= self.MICROSTEPS
            if (style == Raspi_MotorHAT.INTERLEAVE):
                if (dir == Raspi_MotorHAT.FORWARD):
                    self.currentstep += self.MICROSTEPS / 2
                else:
                    self.currentstep -= self.MICROSTEPS / 2

            if (style == Raspi_MotorHAT.MICROSTEP):
                if (dir == Raspi_MotorHAT.FORWARD):
                    self.currentstep += 1
                else:
                    self.currentstep -= 1

                    # go to next 'step' and wrap around
                    self.currentstep += self.MICROSTEPS * 4
                    self.currentstep %= self.MICROSTEPS * 4

                    pwm_a = pwm_b = 0
                if (self.currentstep >= 0) and (self.currentstep <
                                                self.MICROSTEPS):
                    pwm_a = self.MICROSTEP_CURVE[self.MICROSTEPS -
                                                 self.currentstep]
                    pwm_b = self.MICROSTEP_CURVE[self.currentstep]
                elif (self.currentstep >= self.MICROSTEPS) and (
                        self.currentstep < self.MICROSTEPS * 2):
                    pwm_a = self.MICROSTEP_CURVE[self.currentstep -
                                                 self.MICROSTEPS]
                    pwm_b = self.MICROSTEP_CURVE[self.MICROSTEPS * 2 -
                                                 self.currentstep]
                elif (self.currentstep >= self.MICROSTEPS * 2) and (
                        self.currentstep < self.MICROSTEPS * 3):
                    pwm_a = self.MICROSTEP_CURVE[self.MICROSTEPS * 3 -
                                                 self.currentstep]
                    pwm_b = self.MICROSTEP_CURVE[self.currentstep -
                                                 self.MICROSTEPS * 2]
                elif (self.currentstep >= self.MICROSTEPS * 3) and (
                        self.currentstep < self.MICROSTEPS * 4):
                    pwm_a = self.MICROSTEP_CURVE[self.currentstep -
                                                 self.MICROSTEPS * 3]
                    pwm_b = self.MICROSTEP_CURVE[self.MICROSTEPS * 4 -
                                                 self.currentstep]

            # go to next 'step' and wrap around
            self.currentstep += self.MICROSTEPS * 4
            self.currentstep %= self.MICROSTEPS * 4

            # only really used for microstepping, otherwise always on!
            self.MC._pwm.setPWM(self.PWMA, 0, pwm_a * 16)
            self.MC._pwm.setPWM(self.PWMB, 0, pwm_b * 16)

            # set up coil energizing!
            coils = [0, 0, 0, 0]

            if (style == Raspi_MotorHAT.MICROSTEP):
                if (self.currentstep >= 0) and (self.currentstep <
                                                self.MICROSTEPS):
                    coils = [1, 1, 0, 0]
                elif (self.currentstep >= self.MICROSTEPS) and (
                        self.currentstep < self.MICROSTEPS * 2):
                    coils = [0, 1, 1, 0]
                elif (self.currentstep >= self.MICROSTEPS * 2) and (
                        self.currentstep < self.MICROSTEPS * 3):
                    coils = [0, 0, 1, 1]
                elif (self.currentstep >= self.MICROSTEPS * 3) and (
                        self.currentstep < self.MICROSTEPS * 4):
                    coils = [1, 0, 0, 1]
            else:
                step2coils = [[1, 0, 0, 0], [1, 1, 0, 0], [0, 1, 0, 0],
                              [0, 1, 1, 0], [0, 0, 1, 0], [0, 0, 1, 1],
                              [0, 0, 0, 1], [1, 0, 0, 1]]
                coils = step2coils[int(self.currentstep /
                                       (self.MICROSTEPS / 2))]

            #print "coils state = " + str(coils)
            self.MC.setPin(self.AIN2, coils[0])
            self.MC.setPin(self.BIN1, coils[1])
            self.MC.setPin(self.AIN1, coils[2])
            self.MC.setPin(self.BIN2, coils[3])

            return self.currentstep

        def step(self, steps, direction, stepstyle):
            s_per_s = self.sec_per_step
            lateststep = 0

            if (stepstyle == Raspi_MotorHAT.INTERLEAVE):
                s_per_s = s_per_s / 2.0
            if (stepstyle == Raspi_MotorHAT.MICROSTEP):
                s_per_s /= self.MICROSTEPS
                steps *= self.MICROSTEPS
                print(s_per_s, " sec per step")

            for s in range(steps):
                lateststep = self.oneStep(direction, stepstyle)
                time.sleep(s_per_s)

            if (stepstyle == Raspi_MotorHAT.MICROSTEP):
                # this is an edge case, if we are in between full steps, lets just keep going
                # so we end on a full step
                while (lateststep != 0) and (lateststep != self.MICROSTEPS):
                    lateststep = self.oneStep(dir, stepstyle)
                    time.sleep(s_per_s)
예제 #23
0
# recommended for auto-disabling motors on shutdown!
def turnOffMotors():
    mh.getMotor(1).run(Raspi_MotorHAT.RELEASE)
    mh.getMotor(2).run(Raspi_MotorHAT.RELEASE)
    mh.getMotor(3).run(Raspi_MotorHAT.RELEASE)
    mh.getMotor(4).run(Raspi_MotorHAT.RELEASE)


atexit.register(turnOffMotors)

#assign motors to correct pins
LMotor = mh.getMotor(3)
RMotor = mh.getMotor(2)

# Initialise the PWM device using the default address
pwm = PWM(0x6F)

# set frequency, max, and min where servo0=Horiz and servo1=vert
servoMin0 = 155  # Min pulse length out of 4096
servoMid0 = 370
servoMax0 = 585  # Max pulse length out of 4096
servoMin1 = 410  # Min pulse length out of 4096
servoMid1 = 530
servoMax1 = 650  # Max pulse length out of 4096

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

# manually set the servo location using pulse length to middle
HorizAngle = 370
pwm.setPWM(0, 0, HorizAngle)
VertAngle = 530
예제 #24
0
#!/usr/bin/python

from Raspi_PWM_Servo_Driver import PWM
import time

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

# Initialise the PWM device using the default address
# bmp = PWM(0x40, debug=True)
pwm = PWM(0x6F)

servoMin = 150  # Min pulse length out of 4096
servoMax = 600  # Max pulse length out of 4096
servoTEMP = 300


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):
예제 #25
0
from Raspi_PWM_Servo_Driver import PWM

pwm = PWM(0x6F)
pwm.setPWMFreq(60)
pwm.setPWM(0, 0, 350)
예제 #26
0
# setup
GPIO.setmode(GPIO.BCM)
# 25/26 PWM drive speed
GPIO.setup(25, GPIO.OUT) 
GPIO.setup(26, GPIO.OUT)
# setup for pins
GPIO.setup(17, GPIO.OUT)
GPIO.setup(24, GPIO.OUT)
GPIO.setup(23, GPIO.OUT)
GPIO.setup(22, GPIO.OUT)

speed = 40
p=GPIO.PWM(25,speed)
p=GPIO.PWM(26,speed)
pwm=PWM(0x6f)
mh = Raspi_MotorHAT(0x6F)

servoMin = 150
servoMax = 60
S1 = mh.getStepper(200, 1)
S2 = mh.getStepper(200, 2)

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
예제 #27
0
#!/usr/bin/python
#-*- coding:utf-8 -*
from Raspi_PWM_Servo_Driver import PWM
import time
import cv2

pwm = PWM(0x6F)
servoMin = 150  # Min pulse length out of 4096
servoMax = 600  # Max pulse length out of 4096

cap = cv2.VideoCapture(0)

while (True):
    ret, frame = cap.read()
    KeyCode = cv2.waitKey(1)
    KeyCode &= 0xFF
    cv2.imshow("capture", frame)
    if KeyCode == ord('i'):  #up
        pwm.setPWM(0, 0, 600)
        pwm.setPWM(1, 0, 370)
        print "云台抬起!"

    if KeyCode == ord('k'):  #mid
        pwm.setPWM(0, 0, 390)
        pwm.setPWM(1, 0, 370)
        print "云台中立位!"

    if KeyCode == ord('m'):  #down
        pwm.setPWM(0, 0, 210)
        pwm.setPWM(1, 0, 370)
        print "云台收!"
예제 #28
0
class CameraServo:
    def __init__(self, default_h_pos = START_HORIZONTAL_POS, default_v_pos = START_VERTICAL_POS):
        self.pwm = PWM(0x6F, debug=True)
        self.pwm.setPWMFreq(50)
        self.current_horizontal_pos = default_h_pos
        self.current_vertical_pos = default_v_pos
        self.pwm.setPWM(HORIZONTAL_CHANNEL, 0, int(self.current_horizontal_pos))
        self.pwm.setPWM(VERTICAL_CHANNEL, 0, int(self.current_vertical_pos))


    def convert_angle_to_pwm(self, angle):
        return int((servoMax - servoMin) * angle / 180)

    def set_postion(self, horizontal_pos, vertical_pos):
        '''
        The postion is angle
        0 ---->  servoMin
        180 ----> servoMax
        ''' 
        horizontal_pwm = self.convert_angle_to_pwm(horizontal_pos) + 150
        vertical_pwm = self.convert_angle_to_pwm(vertical_pos) + 150
        if horizontal_pwm < servoMin or horizontal_pwm > servoMax:
            print("invaild horizontal_pwm!")
            return
        if vertical_pwm < servoVerticalMin or vertical_pwm > servoVerticalMax:
            print("invaild vertical_pwm")
            return

        print("set horizontal to: %s " % horizontal_pwm)
        print("set vertical to: %s " % vertical_pwm)

        self.pwm.setPWM(HORIZONTAL_CHANNEL, 0, int(horizontal_pwm))
        self.pwm.setPWM(VERTICAL_CHANNEL, 0, int(vertical_pos))

    def update_pos(self, direction, x_or_y, delta_angle):
        '''
        This function is used to update postion by a solid step
        direction: 1, increase; 0, decrease
        x_or_y: 0, horizontal; 1, vertical
        delta_angle: position change in angle
        '''
        delta_pwm = self.convert_angle_to_pwm(delta_angle)
        if x_or_y == 0:
            if direction == 0:
                if self.current_horizontal_pos > servoMin:
                    self.current_horizontal_pos = max(self.current_horizontal_pos - delta_pwm, servoMin)
            elif direction == 1:
                if self.current_horizontal_pos < servoMax:
                    self.current_horizontal_pos = min(self.current_horizontal_pos + delta_pwm, servoMax)
            else:
                print("unknow direction!")
        elif x_or_y == 1:
            if direction == 0:
                if self.current_vertical_pos > servoVerticalMin:
                    self.current_vertical_pos = max(self.current_vertical_pos - delta_pwm, servoMin)
            elif direction == 1:
                if self.current_vertical_pos < servoVerticalMax:
                    self.current_vertical_pos = min(self.current_vertical_pos + delta_pwm, servoMax)
            else:
                print("unknow direction!")
        else:
            print("unknow x_or_y!")
        print("set self.current_horizontal_pos: " + str(self.current_horizontal_pos))
        print("set self.current_vertical_pos: " + str(self.current_vertical_pos))
        self.pwm.setPWM(HORIZONTAL_CHANNEL, 0, int(self.current_horizontal_pos))
        self.pwm.setPWM(VERTICAL_CHANNEL, 0, int(self.current_vertical_pos))
예제 #29
0
파일: car.py 프로젝트: KimUJin3359/IoT_Car
class pollingThread(QThread):
    def __init__(self):
        super().__init__()

    def run(self):
        db = QtSql.QSqlDatabase.addDatabase('QMYSQL')
        db.setHostName("13.209.64.111")
        db.setDatabaseName("IoT_data")
        db.setUserName("ujin")
        db.setPassword("ujin")
        ok = db.open()
        print(ok)

        self.mh = Raspi_MotorHAT(addr=0x6f)
        self.myMotor = self.mh.getMotor(2)
        self.myMotor.setSpeed(100)
        self.speed_value = 100

        self.pwm = PWM(0x6F)
        self.pwm.setPWMFreq(60)

        #self.sense=SenseHat()

        self.BUZZER = 5
        self.RIGHT_LED_F = 23
        self.LEFT_LED_F = 24
        self.RIGHT_LED_R = 8
        self.LEFT_LED_R = 7
        self.ECHO = 21
        self.TRIG = 20

        self.timeF = 0
        self.camera = picamera.PiCamera()

        GPIO.setmode(GPIO.BCM)
        GPIO.setwarnings(False)
        GPIO.setup(self.BUZZER, GPIO.OUT)
        GPIO.setup(self.RIGHT_LED_F, GPIO.OUT)
        GPIO.setup(self.LEFT_LED_F, GPIO.OUT)
        GPIO.setup(self.RIGHT_LED_R, GPIO.OUT)
        GPIO.setup(self.LEFT_LED_R, GPIO.OUT)
        GPIO.setup(self.TRIG, GPIO.OUT)
        GPIO.setup(self.ECHO, GPIO.IN)

        while True:
            if (self.timeF == 10):
                now = datetime.datetime.now()
                self.camera.capture('./img/' + now.strftime('%Y%m%d%H%M%S') +
                                    '.jpg')
                self.timeF = 0
            time.sleep(0.1)
            self.getQuery()
            # self.setQuery()
            self.timeF += 1

    def setQuery(self):
        '''
        self.query.prepare("insert into sensing (time, num1, num2, num3, meta_string, is_finish) values (:time, :num1, :num2, :num3, :meta, :finish)")
        time = QDateTime().currentDateTime()
        self.query.bindValue(":time", time)
        self.query.bindValue(":num1", self.sensor.distance)
        self.query.bindValue(":num2", "")
        self.query.bindValue(":num3", "")
        self.query.bindValue(":meta", "")
        self.query.bindValue(":finish", 0)
        self.query.exec()
        '''

    def getQuery(self):
        query = QtSql.QSqlQuery(
            "select * from command order by time desc limit 1")
        query.next()

        cmdTime = query.record().value(0)
        cmdType = query.record().value(1)
        cmdArg = query.record().value(2)
        is_finish = query.record().value(3)

        if is_finish == 0:
            query = QtSql.QSqlQuery(
                "update command set is_finish=1 where is_finish=0")
            if cmdType == "go": self.go()
            if cmdType == "back": self.back()
            if cmdType == "left": self.left()
            if cmdType == "right": self.right()
            if cmdType == "mid": self.mid()
            if cmdType == "stop": self.stop()
            if cmdType == "speed":
                self.speed_value = int(cmdArg)
                self.speed()
            if cmdType == "buzz": self.buzz()

    def buzz(self):
        GPIO.output(self.BUZZER, True)
        time.sleep(0.5)
        GPIO.output(self.BUZZER, False)

    def speed(self):
        print("MOTOR SPEED SET")
        self.myMotor.setSpeed(self.speed_value)

    def go(self):
        print("MOTOR GO")
        self.myMotor.setSpeed(self.speed_value)
        self.myMotor.run(Raspi_MotorHAT.FORWARD)

    def back(self):
        print("MOTOR BACK")
        self.myMotor.setSpeed(self.speed_value)
        self.myMotor.run(Raspi_MotorHAT.BACKWARD)

    def stop(self):
        print("MOTOR STOP")
        self.myMotor.run(Raspi_MotorHAT.RELEASE)

    def left(self):
        print("MOTOR LEFT")
        self.pwm.setPWM(0, 0, 230)
        GPIO.output(self.LEFT_LED_F, True)
        GPIO.output(self.RIGHT_LED_F, False)
        GPIO.output(self.LEFT_LED_R, True)
        GPIO.output(self.RIGHT_LED_R, False)

    def mid(self):
        print("MOTOR MID")
        self.pwm.setPWM(0, 0, 325)
        GPIO.output(self.LEFT_LED_F, False)
        GPIO.output(self.RIGHT_LED_F, False)
        GPIO.output(self.LEFT_LED_R, False)
        GPIO.output(self.RIGHT_LED_R, False)

    def right(self):
        print("MOTOR RIGHT")
        self.pwm.setPWM(0, 0, 390)
        GPIO.output(self.LEFT_LED_F, False)
        GPIO.output(self.RIGHT_LED_F, True)
        GPIO.output(self.LEFT_LED_R, False)
        GPIO.output(self.RIGHT_LED_R, True)
예제 #30
0
import time
import atexit
from Raspi_MotorHAT import Raspi_MotorHAT, Raspi_DCMotor  # DC motor driver
from Raspi_PWM_Servo_Driver import PWM  # Servo Control

#! Global Variables and magic numbers
SERVOMIN = 150  # Min pulse length, us (tick 184/4096)
SERVOMAX = 550  # Max pulse length, us  (tick 430/4096)
servoMid = SERVOMAX - ((SERVOMAX - SERVOMIN) / 2)  # Midpoint pulse length, us
FREQUENCY = 50  # frequency length, Hz

#! Initialization
servo = PWM(0x6F)
servo.setPWMFreq(FREQUENCY)  # Set frequency
motorControl = Raspi_MotorHAT(addr=0x6f)


def turnOffMotors():
    motorControl.getMotor(1).run(Raspi_MotorHAT.RELEASE)
    motorControl.getMotor(2).run(Raspi_MotorHAT.RELEASE)
    motorControl.getMotor(3).run(Raspi_MotorHAT.RELEASE)
    motorControl.getMotor(4).run(Raspi_MotorHAT.RELEASE)


def map(x, in_min, in_max, out_min, out_max):
    return int((x - in_min) * (out_max - out_min) / (in_max - in_min) +
               out_min)


def pulseWidthCal(inputAngle):
    pulse_wide = map(inputAngle, 0, 180, SERVOMIN, SERVOMAX)
예제 #31
0
import cv2
import sys
import imutils
import time
from Raspi_PWM_Servo_Driver import PWM

#! Global Variables

#! Robot Initialization
#* Motor initialization

pwm = PWM(0x6F)  #Hat setting, do not change!!!
pwm.setPWMFreq(50)  # Set frequency to 50Hz, mandatory!!!
# Following are magic numbers, Do not change!!!
G_FREQUENCY = 50
#Pan setting
PAN_CHANNEL = 0
SERVOMIN_PAN = 150  #Pan Minimal pulse
SERVOMup1AX_PAN = 550  #Pan Max pulse
SERVONEUTRAL_PAN = 350
#Tilt Setting
TILT_CHANNEL = 1
SERVOMIN_TILT = 150
SERVOMAX_TILT = 400
SERVONEUTRAL_TILT = 200


def map(x, in_min, in_max, out_min, out_max):
    return int((x - in_min) * (out_max - out_min) / (in_max - in_min) +
               out_min)
예제 #32
0
 def Setup_Servo(self):
     return PWM(self.ADDRESS)
예제 #33
0
import json
from Raspi_PWM_Servo_Driver import PWM
from Raspi_MotorHAT import Raspi_MotorHAT, Raspi_DCMotor
import time
import requests
import serial as serial

pwm = PWM(0x06F)
mh = Raspi_MotorHAT(addr=0x6f)
motor = mh.getMotor(1)

servoMin = 0  # Min pulse length out of 4096
servoMax = 650  # Max pulse length out of 4096


def setAngle(angle):
    if angle > 45 or angle < 0:
        pwm.setPWM(1, 0, 0)
        print("error in angle number")
        return
    ticks = int(angle * 5.55) + 210
    if ticks < 210 or ticks > 460:
        pwm.setPWM(1, 0, 0)
        print("error in angle number")
        return
    pwm.setPWMFreq(50)
    pwm.setPWM(1, 0, ticks)


def getShotStats():
    r = requests.get(
예제 #34
0
from Raspi_PWM_Servo_Driver import PWM

pwm = PWM(0x6F)
pwm.setPWMFreq(50)
pwm.setPWM(0, 0, 400)
예제 #35
0
#Variables para la conexion Cliente-Servidor
serverSocket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
port = 9999
serverSocket.bind(
    ('', port)
)  #de esta forma el servidor escucha peticiones de cualquier PC en la red
#setting amount of requests
requestsNumber = 5
serverSocket.listen(requestsNumber)

# create a default object, no changes to I2C address or frequency
mh = Raspi_MotorHAT(addr=0x6f)

#Parametros para el servo motor
pwm = PWM(0x6F)

servoMin = 180  # Min pulse length out of 4096
servoMax = 600  # Max pulse length out of 4096

minAngle = 140
maxAngle = 230

minSpeed = 0
maxSpeed = 255
''' Ajuste matematico para variacion lineal del angulo de
 giro del servo
 
 p1 = (0   , 180)
 p2 = (180 , 600)