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
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 __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
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 __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)
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
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))
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]
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 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)
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')
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]
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);
#!/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)
#!/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
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)
# 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
#!/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)
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)
def __init__(self, debug=False):
self.debug = debug
self.pwm = PWM(0x6F, debug=False)
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)
# 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
#!/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):
from Raspi_PWM_Servo_Driver import PWM pwm = PWM(0x6F) pwm.setPWMFreq(60) pwm.setPWM(0, 0, 350)
# 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
#!/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 "云台收!"
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))
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)
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)
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)
def Setup_Servo(self):
return PWM(self.ADDRESS)
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(
from Raspi_PWM_Servo_Driver import PWM pwm = PWM(0x6F) pwm.setPWMFreq(50) pwm.setPWM(0, 0, 400)
#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)