def test_motor():
FORWARD_PIN = 26
BACKWARD_PIN = 19
motionMotor = Motor(GPIO, lambda secs: sleep(secs), FORWARD_PIN,
BACKWARD_PIN)
motionMotor.forward(1)
sleep(2)
motionMotor.backward(1)
class MotorInterface:
#Instanciates the object
def __init__(self):
self.motor = Motor()
self.motor.enable()
'''
Goes forward x seconds
'''
def forward(self, tsec):
self.motor.forward()
time.sleep(tsec)
self.motor.stop()
'''
Goes left x seconds
'''
def left(self, tsec):
self.motor.left()
time.sleep(tsec)
self.motor.stop()
'''
Goes right x seconds
'''
def right(self, tsec):
self.motor.right()
time.sleep(tsec)
self.motor.stop()
'''
Goes back x seconds
'''
def back(self, tsec):
self.motor.backward()
time.sleep(tsec)
self.motor.stop()
'''
Stops and disables the motor
'''
def disable(self):
self.motor.stop()
self.motor.disable()
'''
Enables the motor
'''
def enable(self):
self.motor.enable()
class Motors:
def __init__(self, pinIN1, pinIN2, pinIN3, pinIN4):
self.motorLeft = Motor(pinIN1, pinIN2)
self.motorRight = Motor(pinIN3, pinIN4)
def forward(self):
self.motorLeft.forward()
self.motorRight.forward()
def backward(self):
self.motorLeft.backward()
self.motorRight.backward()
def left(self):
self.motorLeft.backward()
self.motorRight.forward()
def right(self):
self.motorLeft.forward()
self.motorRight.backward()
def stop(self):
self.motorLeft.stop()
self.motorRight.stop()
# Socket server in python using select function
import socket
import select
import time
import sys
from motor import Motor
motor = Motor()
motor.forward()
motor.backward()
from oled import *
oled = Oled()
oled.head()
oled.square()
oled.foot("START")
CONNECTION_LIST = [] # list of socket clients
RECV_BUFFER = 1024 #4096 # Advisable to keep it as an exponent of 2
PORT = 8888
server_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
# this has no effect, why ?
server_socket.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
server_socket.bind(("0.0.0.0", PORT))
server_socket.listen(10)
# Add server socket to the list of readable connections
CONNECTION_LIST.append(server_socket)
class Powertrain:
"""Represents a pair of motors. Uses the Motor class to control them.
"""
def __init__(self, left_wheel_forward, left_wheel_backward,
left_wheel_enable, right_wheel_forward, right_wheel_backward,
right_wheel_enable):
"""Constructor.
Args:
left_wheel_forward (int): GPIO pin connected to left motor's
forward pin.
left_wheel_backward (int): GPIO pin connected to left motor's
backward pin.
left_wheel_enable (int): GPIO pin connected to left motor's enable
pin.
right_wheel_forward (int): GPIO pin connected to right motor's
forward pin.
right_wheel_backward (int): GPIO pin connected to right motor's
backward pin.
right_wheel_enable (int): GPIO pin connected to right motor's
enable pin.
"""
# set motors
self.left = Motor(left_wheel_forward, left_wheel_backward,
left_wheel_enable)
self.right = Motor(right_wheel_forward, right_wheel_backward,
right_wheel_enable)
def forward(self, duty_cycle):
"""Drive the powertrain forward.
Args:
duty_cycle (float): The duty cycle to run the motors at.
"""
# apply to both motors
self.left.forward(duty_cycle)
self.right.forward(duty_cycle)
def reverse(self, duty_cycle):
"""Drive the powertrain backward.
Args:
duty_cycle (float): The duty cycle to run the motors at.
"""
# apply to both motors
self.left.backward(duty_cycle)
self.right.backward(duty_cycle)
def turn(self,
left_duty_cycle,
right_duty_cycle,
left_forward=True,
right_forward=True):
"""Drive motor speeds separately to turn.
Args:
left_duty_cyle (float): The duty cyle to run the left motor at.
right_duty_cycle (float): The duty cycle to run the right motor at.
left_forward (boolean, optional): Flag for the left motor to go
forward. Defaults to True.
right_forward (boolean, optional): Flag for the right motor to go
forward. Defaults to True.
"""
#print("LDC: {0}, RDC: {1}".format(left_duty_cycle, right_duty_cycle))
# if-else to use forward/backward
if (left_forward):
self.left.forward(left_duty_cycle)
else:
self.left.backward(left_duty_cycle)
if (right_forward):
self.right.forward(right_duty_cycle)
else:
self.right.backward(right_duty_cycle)
def turn_left(self, max_duty_cycle, intensity=50.0, forward=True):
"""Drive the motors to turn left.
Args:
max_duty_cycle (float): The duty cycle to run the right motor at.
The left motor runs at a portion of this.
intensity (float, optional): The intensity to turn left. The value
is clamped between 100 and 0. A greater intensity turns harder.
Defaults to 50.0.
forward (boolean, optional): Flag for spinning motors forward.
Defaults to True.
"""
# get min_duty_cycle
min_duty_cycle = get_new_duty_cycle(max_duty_cycle, intensity)
# pass to turn
self.turn(min_duty_cycle, max_duty_cycle, forward, forward)
def turn_right(self, max_duty_cycle, intensity=50.0, forward=True):
"""Drive the motors to turn right.
Args:
max_duty_cycle (float): The duty cycle to run the left motor at.
The right motor runs at a portion of this.
intensity (float, optional): The intensity to turn right. The value
is clamped between 100 and 0. A greater intensity turns harder.
Defaults to 50.0.
forward (boolean, optional): Flag for spinning motors forward.
Defaults to True.
"""
# get min_duty_cycle
min_duty_cycle = get_new_duty_cycle(max_duty_cycle, intensity)
# pass to turn
self.turn(max_duty_cycle, min_duty_cycle, forward, forward)
def turn_intensity(self, max_duty_cycle, intensity, forward=True):
"""Drive the motors to turn based on intensity and its sign.
Args:
max_duty_cycle (float): The duty cycle to run the faster motor at.
The other motor runs at a portion of this.
intensity (float): The intensity to turn. The sign of the intensity
affects the turn direction (e.g. negative for left, positive
for right). The absolute value is clamped between 100 and 0. A
greater intensity turns harder. If intensity is 0, this
function drives motors equally.
forward (boolean, optional): Flag for spinning motors forward.
Defaults to True.
"""
# need to multiply intensity by -1 if intensity is negative
if (intensity < 0):
self.turn_left(max_duty_cycle,
intensity=(-1 * intensity),
forward=forward)
else:
self.turn_right(max_duty_cycle,
intensity=intensity,
forward=forward)
def pivot(self, duty_cycle, clockwise=False):
"""Drive the motors to run opposite of each other to pivot.
Args:
duty_cycle (float): The duty cycle to run the motors at.
clockwise (boolean, optional): Flag for pivoting clockwise.
Defaults to False.
"""
if (clockwise):
self.turn(duty_cycle, duty_cycle, right_forward=False)
else:
self.turn(duty_cycle, duty_cycle, left_forward=False)
def stop(self):
"""Stop the motors.
"""
# TODO should this call brake() for a short time then call off()?
self.left.off()
self.right.off()
def cleanup(self):
"""Cleanup GPIO pins for the motors.
Calling this releases the motors, so further use of the powertrain will
not be possible.
"""
# apply to both motors
self.left.cleanup()
self.right.cleanup()
class StreamDrive(object):
def __init__(self):
# Set up GPIO for RPi
GPIO.setmode(GPIO.BOARD)
forwardPin = 7
backwardPin = 11
leftPin = 13
rightPin = 15
controlStraightPin = 29
self.motor = Motor(forwardPin, backwardPin, controlStraightPin,
leftPin, rightPin)
self.sensor = UltrasonicSensor()
# Initialize server for receiving driving instructions
self.server_socket = socket.socket()
self.server_socket.bind(('0.0.0.0', 8000))
self.server_socket.listen(0)
self.server_connection, self.client_address = self.server_socket.accept(
)
print("Connection from: ", self.client_address)
# Initialize client for streaming camera pictures
self.client_socket = socket.socket()
self.client_socket.connect(('192.168.1.11', 8000))
# Make a file-like object out of the client connection
self.client_connection = self.client_socket.makefile('wb')
# Start the stream
self.stream()
def stream(self):
try:
# Initialize camera and allow some warmup time, create stream
self.camera = picamera.PiCamera()
self.camera.resolution = (320, 240)
self.camera.framerate = 10
time.sleep(2)
start = time.time()
stream = io.BytesIO()
for frame in self.camera.capture_continuous(stream,
'jpeg',
use_video_port=True):
# Write the length of the capture to the stream
# and flush to make sure it gets sent
self.client_connection.write(struct.pack('<L', stream.tell()))
self.client_connection.flush()
# Rewind the stream and send image data through
stream.seek(0)
self.client_connection.write(stream.read())
# Reset stream for next capture
stream.seek(0)
stream.truncate()
# Write sensor data to server
distance = self.sensor.measure_average()
self.server_connection.send(struct.pack("f", distance))
# Get the pressed key
key = self.server_connection.recv(1024).decode()
if (key == "w"):
self.motor.forward()
elif (key == "s"):
self.motor.backward()
elif (key == "a"):
self.motor.forward_left()
elif (key == "d"):
self.motor.forward_right()
elif (key == "space"):
self.motor.stop()
elif (key == "x"):
print("Program terminated")
break
else:
self.motor.stop()
key = ""
# Write a length of 0 to the stream to signal we're done
self.client_connection.write(struct.pack('<L', 0))
finally:
self.client_connection.close()
self.client_socket.close()
self.server_connection.close()
self.server_socket.close()
GPIO.cleanup()
from motor import Motor
from getch import _GetchUnix
import time
import sys
import termios
import tty
var = 'a'
getch = _GetchUnix()
motor = Motor()
while var != 'q':
print('q-quit,f-forward,b-backward,l-turnLeft,r-turnRight')
var = getch()
if var == 'f':
motor.forward()
print('Move forward')
if var == 'b':
motor.backward()
print('Move backward')
if var == 'l':
motor.turnLeft()
print('Turn left')
if var == 'r':
motor.turnRight()
print('Turn right')
class CAR():
#控制左右转向时间，根据转弯情况修改
delay_tr = 0.65
delay_tl = 0.65
delay_30 = 1
#开启自动避障后的转弯时间
auto_run_tr = 0.5
auto_run_tl = 0.5
#录音采样参数
NUM_SAMPLES = 2000
flag = 0
saveCount = 0
def __init__(self):
config = configparser.ConfigParser()
config.read("config.ini")
IN_1 = config.getint("car","IN1")
IN_2 = config.getint("car","IN2")
IN_3 = config.getint("car","IN3")
IN_4 = config.getint("car","IN4")
self.change_speed = ChangeSpeed()
self.motor = Motor(IN_1, IN_2, IN_3, IN_4)
self.sense = Sense()
self.qaudio = QAudio(4000)
self.led_light = LED_Light()
# self.audio_record = RecordThread('command.wav')
# self.audio_record.setDaemon(True)
def CarMove(self,direction,loop):
if direction == 'F':
self.motor.forward()
elif direction == 'B':
self.motor.backward()
elif direction == 'R':
self.motor.turnright()
elif direction == 'L':
self.motor.turnleft()
elif direction == 'A':
self.autorun(loop)
elif direction == 'S':
self.motor.stop()
elif direction == 'E':
self.motor.gpio_release()
exit()
else:
print("The input error! please input:F,B,L,R,S")
#下面的代码都是用于避障的函数，里面所有的loop都是之后要用到的。
#the methods is used to strategy and run
def backx(self,loop):
while loop:
self.motor.backward()
time.sleep(0.5)
self.motor.stop()
l_d = self.sense.ask_distance_l()
r_d = self.sense.ask_distance_r()
if r_d > 30 and r_d >= l_d:
#get a safe distance
self.motor.backward()
time.sleep(0.6)
#turn right 90
self.motor.turnright()
time.sleep(self.auto_run_tr)
break
if l_d > 30 and l_d > r_d:
#get a safe distance
self.motor.backward()
time.sleep(0.6)
#turn left 90
self.motor.turnleft()
time.sleep(self.auto_run_tl)
break
#run and strategy
def autorun(self,loop):
self.audio_record = RecordThread()
self.audio_record.setDaemon(True)
self.audio_record.start()
try:
while True:
time.sleep(0.001)
self.change_speed.speed_change(80,80)
#get obstacle type
strategy = self.sense.detection()
if strategy == "Fgo":
self.motor.forward()
elif strategy == "Bgo":
self.backx(loop)
elif strategy == "Lgo":
#turn left 90
self.motor.turnleft()
time.sleep(self.auto_run_tr)
elif strategy == "Rgo":
#turn left 90
self.motor.turnright()
time.sleep(self.auto_run_tr)
with open('command.txt','r',encoding='utf-8') as f:
command = f.read()
if command.find('停') != -1 or command.find('止') != -1:
self.led_light.led(1,True)
self.led_light.led(2,False)
print("自动避障停止")
break
except KeyboardInterrupt:
print("")
print("stop the car")
self.motor.gpio_release()
exit()
# Analysis command 通过操作文件识别命令
def recognizeCommand(self, filename):
enStop = 0
enRun = 1
enBack = 2
enLeft = 3
enRight = 4
autoRun = 5
car_State = 0
breakRecordFlag=0
self.motor.stop()
file = filename
f = open(file,'r',)
out = f.read()
command = out
f.close() # close file aviod exception
#print(command)
if command.find('前') != -1 :#or command.find('向') != -1:
print("向前")
car_State = 1
elif command.find('后') != -1 :#or command.find('向') != -1:
print ("向后")
car_State = 2
elif command.find('左') != -1 :#and command.find('转') != -1:
print ('左转弯')
car_State = 3
elif command.find('右') != -1 :#and command.find('转') != -1:
print ('右转弯')
car_State = 4
elif command.find('自') != -1 and command.find('动') != -1 or command.find('避') != -1 or command.find('障') != -1:
car_State = 5
print ('自动避障')
elif command.find('拜') != -1:
breakRecordFlag = 1
elif command.find('识') != -1 and command.find('别') != -1 and command.find('错') != -1 and command.find('误') != -1:
print ('语音识别出错')
else:
print ('指令错误')
car_State = 0
#向前
if car_State == enRun:
print('向前！')
self.motor.forward()
self.change_speed.speed_change(80,80)
time.sleep(self.delay_30)
car_State = 0
#向后
elif car_State == enBack :
self.motor.backward()
self.change_speed.speed_change(80,80)
time.sleep(self.delay_30)
car_State = 0
#左转弯
elif car_State == enLeft :
self.motor.turnleft()
self.change_speed.speed_change(80,80)
time.sleep(self.delay_tr)
car_State = 0
#右转弯
elif car_State == enRight :
self.motor.turnright()
self.change_speed.speed_change(80,80)
time.sleep(self.delay_tr)
car_State = 0
#自动避障
elif car_State == autoRun:
self.led_light.led(1,False)
self.led_light.led(2,True)
self.autorun(True)
#stop
elif car_State == enStop:
self.motor.stop()
else:
self.motor.stop()
return breakRecordFlag
#调用科大讯飞语音识别SDK，识别率一般
def monitor(self):
try:
while True:
self.qaudio.sound_detect()
breakFlag = self.recognizeCommand('result.txt') #Analysis command
self.motor.stop()
if breakFlag == 1:
breakFlag = 0
break
except KeyboardInterrupt:
print('Car Stop!')
self.qaudio.close()
self.motor.gpio_release()
GPIO.cleanup()
exit()
#save data to filename
def save_wave_file(self,filename,data):
wf = wave.open(filename,'wb')
wf.setnchannels(1) #set channel
wf.setsampwidth(2) #采样字节 1 or 2
wf.setframerate(16000) #采样频率 8K or 16K
wf.writeframes(b"".join(data))
wf.close()
# baidu SST check the voice volumn record 5s
def monitor_baidu(self):
self.led_light.led(1,True)
pa = PyAudio()
stream = pa.open(format = paInt16,
channels=1,
rate=16000,
input=True,
frames_per_buffer=self.NUM_SAMPLES)
print('开始缓存录音')
audioBuffer = []
rec = []
audioFlag = False
timeFlag = False
try:
while True:
data = stream.read(self.NUM_SAMPLES,exception_on_overflow = False)
audioBuffer.append(data) #录音源文件
audioData = np.fromstring(data,dtype=np.short) #字符串创建矩阵
largeSampleCount = np.sum(audioData > 2000)
temp = np.max(audioData)
print(temp)
if temp > 3000 and audioFlag == False: #3000 according different mic
audioFlag = True #开始录音
print("检测到语音信号，开始录音")
begin = time.time()
print(temp)
if audioFlag:
end = time.time()
if end-begin > 5:
timeFlag = 1 #5s录音结束
if largeSampleCount > 20:
self.saveCount = 3
else:
self.saveCount -=1
if self.saveCount <0:
self.saveCount =0
if self.saveCount >0:
rec.append(data)
else:
if len(rec) >0 or timeFlag:
self.save_wave_file('result.wav',rec) # 保存检测的语音
voice.identify_synthesize('result.wav') # 调用百度语音实现语音识别和语音合成
rec = []
audioFlag = False
timeFlag = 0
breakFlag=self.recognizeCommand('result.txt') #Analysis command
self.motor.stop()
if breakFlag == 1:
breakFlag = 0
break
print("The end!")
stream.stop_stream()
stream.close()
pa.terminate()
GPIO.cleanup()
sys.exit()
except KeyboardInterrupt:
stream.stop_stream()
stream.close()
pa.terminate()
GPIO.cleanup()
sys.exit()
rack = 0
column = 0
step = 0
print(
"Commands: exit, zero, rel (release), step, cam (camera), col (column), pos (position)"
)
while (order != "exit"):
order = input("Wus poppin jimbo? ")
if (order == "zero"):
motor.returnHome()
elif (order == "rel"):
motor.release()
elif (order == "step"):
step = int(input("Input a step: "))
if (step < 0):
motor.backward(-1 * step)
else:
motor.forward(step)
elif (order == "cam"):
rack = int(input("Input a rack: "))
motor.moveToRackForCamera(rack)
elif (order == "col"):
rack = int(input("Input a rack: "))
column = int(input("Input a column: "))
motor.moveToTube(rack, column)
elif (order == "pos"):
print(motor.position)
elif (order == "test"):
motor.test()
if rotation == 'left':
angle += 1
elif rotation == 'right':
angle -= 1
robot.y += y_change * math.cos(math.pi * angle / 180)
robot.x += y_change * math.sin(math.pi * angle / 180)
drawDivider(945, 0, (255, 255, 255))
robot.draw(angle)
if x_change > 0:
pass
#fl.left()
if y_change > 0:
fl.backward()
elif y_change < 0:
fl.forward()
else:
fl.stop()
barrierX = 740
barrierY = 700
#barrierX = 780
#barrierY = 600
# making a copy of the old center of the rectangle
old_center = robot.center
# defining angle of the rotation
#rot = (rot + rot_speed) % 360
gpio.setmode(gpio.BCM) # Pins left_a = 27 left_b = 22 right_a = 5 right_b = 6 left_motor = Motor(gpio, uid="left", pin_a=left_a, pin_b=left_b) right_motor = Motor(gpio, uid="right", pin_a=right_a, pin_b=right_b) for i in range(90): left_motor.forward() right_motor.forward() sleep(0.25) left_motor.stop() right_motor.stop() sleep(1) for i in range(90): left_motor.backward() right_motor.backward() sleep(0.25) left_motor.stop() right_motor.stop() gpio.cleanup() print "All done!"