def test():
gpio.setmode(gpio.BCM)
distance_sensor = DistanceSensor()
motor = Motor(input1=22, input2=27, input3=17, input4=4)
while True:
motor.forward()
def driving_compass_test():
motor = Motor()
motor.forward()
time.sleep(1)
motor.stop()
currentDirection = get_heading()
print("current direction: " + str(currentDirection))
finalDirection = currentDirection + 90 if currentDirection + 90 < 360 else currentDirection + 90 - 360
print("final direction: " + str(currentDirection))
try:
motor.turnRight()
while True:
currentDirection = get_heading()
print("current direction: " + str(currentDirection))
if currentDirection >= finalDirection:
motor.stop()
motor.__del__()
currentDirection = get_heading()
print("current direction: " + str(currentDirection))
break
except KeyboardInterrupt:
motor.stop()
motor.__del__()
def test0(dc):
m1 = Motor(LFP, LBP, LEP);
m2 = Motor(RFP, RBP, REP);
m1.forward(dc[0]);
time.sleep(2);
m1.off();
m2.forward(dc[0]);
time.sleep(2);
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()
def main():
gpio.setmode(gpio.BCM)
motor1 = Motor(input1 = 22 , input2 = 27 , input3 = 17 , input4 = 4)
for i in range(3):
print("Count :{}".format(i))
motor1.forward()
time.sleep(1.0)
motor1.stop()
motor1.reverse()
time.sleep(1.0)
motor1.stop()
time.sleep(0.5)
gpio.cleanup()
def main():
gpio.setmode(gpio.BCM)
distance_sensor = DistanceSensor()
motor = Motor(input1=22, input2=27, input3=17, input4=4)
try:
while True:
distance = distance_sensor.get_distance()
if distance is None:
print("None")
sleep(DISTANCE_SLEEP)
continue
if distance_sensor.is_in_range(distance) == False:
sleep(DISTANCE_SLEEP)
continue
if distance_sensor.is_far(distance):
print("Forward")
motor.forward()
sleep(DISTANCE_SLEEP)
motor.stop()
continue
if distance_sensor.is_close(distance):
print("Backword")
motor.reverse()
sleep(DISTANCE_SLEEP)
motor.stop()
continue
print("....")
sleep(DISTANCE_SLEEP)
except KeyboardInterrupt:
gpio.cleanup()
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()
from motor import Motor
from automobile import Automobile
fl = Motor(4)
fr = Motor(17)
br = Motor(22)
bl = Motor(27)
robot = Automobile(fr, br, fl, bl)
while True:
fl.forward()
#robot.drive()
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
motor1 = Motor(18, 4, 17, 23, 24)
motor2 = Motor(18, 22, 27, 18, 25)
if __name__ == "__main__":
while True:
delay = raw_input("Delay between steps (milliseconds, or -1 to quit)? ")
if delay == "-1":
break
motor1.setDelay(int(delay))
motor2.setDelay(int(delay))
steps = raw_input("How many steps forward? ")
motor1.forward(int(steps))
motor2.forward(int(steps))
motor1.backwards(int(steps))
motor2.backwards(int(steps))
motor1.shutdown()
motor2.shutdown()
imu.setGyroEnable(True)
imu.setAccelEnable(True)
imu.setCompassEnable(True)
poll_interval = imu.IMUGetPollInterval()
print("Recommended Poll Interval: %dmS\n" % poll_interval)
motor = Motor()
try:
while True:
if imu.IMURead():
data = imu.getIMUData()
print(data["accel"])
timeout = time.time() + 1
motor.forward()
break
# x, y, z = imu.getFusionData()
# print("%f %f %f" % (x,y,z))
time.sleep(poll_interval * 1.0 / 1000.0)
while True:
if imu.IMURead():
data = imu.getIMUData()
print(data["accel"])
if time.time() > timeout:
motor.stop()
break
time.sleep(0.2)
except KeyboardInterrupt:
class Wheelie(Node):
'''Wheelie node suitable for a RPi robot with two PWM driven motors
Creates listener on /command to accept string-style commands.
Creates listener on /cmd_vel to accept twist messages.
Creates listener on /joy to accept xbox joystick input.
Attributes
----------
speed : float
Speed along the X axis in meters per second; positive is
forward and negative is backward
spin : float
Rotation about the pivot point in radians per second; positive
is clockwise when viewed from above (right spin)
Methods
-------
stop()
Stop all movement of the wheelie
'''
def __init__(self,
name,
pinRightFwd,
pinRightRev,
pinLeftFwd,
pinLeftRev,
wheel_diameter=.065,
wheel_base=0.15,
left_max_rpm=200.0,
right_max_rpm=200.0,
frequency=20):
"""
Parameters
----------
name: str
The node name that will be used for this robot; defaults
to "wheelie"
pinRightFwd : int
The RaspPi GPIO pin that goes high to create forward motion
on the right wheel
pinRightRev : int
The RaspPi GPIO pin that goes high to create reverse motion
on the right wheel
pinLeftFwd : int
The RaspPi GPIO pin that goes high to create forward motion
on the right wheel
pinLeftRev : int
The RaspPi GPIO pin that goes high to create reverse motion
on the right wheel
wheel_diameter : float
The diameter of the wheels in meters
wheel_base : float
The distance between the center of the wheels in meters
left_max_rpm : float
The number of revolutions per minute made by the left motor
when running at 100% power
right_max_rpm : float
The number of revolutions per minute made by the left motor
when running at 100% power
frequency : int
The frequency in Hz used to control the PWM motors
"""
super().__init__(name)
self._frequency = frequency
self._left_max_rpm = left_max_rpm
self._right_max_rpm = right_max_rpm
self._wheel_diameter = wheel_diameter
self._wheel_base = wheel_base
self._rightWheel = Motor(pinRightFwd, pinRightRev, frequency)
self._leftWheel = Motor(pinLeftFwd, pinLeftRev, frequency)
self.speed = 0.0
self.spin = 0.0
self.close = 0.30 #start slowing down when this close
self.stop = 0.10 #no forward motion when this close
self.distance = 0.0
self._command_subscription = self.create_subscription(
String, 'command', self._command_callback, 10)
self._cmd_vel_subscription = self.create_subscription(
Twist, 'cmd_vel', self._cmd_vel_callback, 2)
self._joy_subscription = self.create_subscription(
Joy, 'joy', self._joy_callback, 5)
self._range_subscription = self.create_subscription(
Range, 'range', self._range_callback, 5)
def stop(self):
self._leftWheel.stop()
self._rightWheel.stop()
def max_speed(self):
'''Speed in meters per second at maximum RPM'''
rpm = (self._left_max_rpm + self._right_max_rpm) / 2.0
mps = rpm * math.pi * self._wheel_diameter / 60.0
return mps
def max_twist(self):
'''Rotation in radians per second at maximum RPM'''
return self.max_speed() / self._wheel_diameter
def _forward(self, speed=100):
self._rightWheel.forward(speed)
self._leftWheel.forward(speed)
def _reverse(self, speed=100):
self._rightWheel.reverse(speed)
self._leftWheel.reverse(speed)
def _left(self, speed=100):
self._rightWheel.reverse(speed)
self._leftWheel.forward(speed)
def _right(self, speed=100):
self._rightWheel.forward(speed)
self._leftWheel.reverse(speed)
def _command_callback(self, msg):
command = msg.data
if command == 'forward':
self._forward()
elif command == 'reverse':
self._reverse()
elif command == 'left':
self._left()
elif command == 'right':
self._right()
elif command == 'stop':
self.stop()
else:
print('Unknown command, stopping instead')
self.stop()
def _joy_callback(self, msg):
'''Translate XBox buttons into speed and spin
Just use the left joystick (for now):
LSB left/right axes[0] +1 (left) to -1 (right)
LSB up/down axes[1] +1 (up) to -1 (back)
LB buttons[5] 1 pressed, 0 otherwise
'''
if abs(msg.axes[0]) > 0.10:
self.spin = msg.axes[0]
else:
self.spin = 0.0
if abs(msg.axes[1]) > 0.10:
self.speed = msg.axes[1]
else:
self.speed = 0.0
if msg.buttons[5] == 1:
self.speed = 0
self.spin = 0
self._set_motor_speeds()
def _cmd_vel_callback(self, msg):
self.speed = msg.linear.x
self.spin = msg.angular.z
self._set_motor_speeds()
def _range_callback(self, msg):
self.distance = msg.range
self._set_motor_speeds()
def _set_motor_speeds(self):
#TODO: inject a stop() if no speeds seen for a while
#
# Scary math ahead.
#
# First figure out the speed of each wheel based on spin: each wheel covers
# self._wheel_base meters in one radian, so the target speed for each wheel
# in meters per sec is spin (radians/sec) times wheel_base divided by
# wheel_diameter
#
right_twist_mps = self.spin * self._wheel_base / self._wheel_diameter
left_twist_mps = -1.0 * self.spin * self._wheel_base / self._wheel_diameter
#
# Now add in forward motion.
#
left_mps = self.speed + left_twist_mps
right_mps = self.speed + right_twist_mps
#
# Convert meters/sec into RPM: for each revolution, a wheel travels pi * diameter
# meters, and each minute has 60 seconds.
#
left_target_rpm = (left_mps * 60.0) / (math.pi * self._wheel_diameter)
right_target_rpm = (right_mps * 60.0) / (math.pi *
self._wheel_diameter)
#
left_percentage = (left_target_rpm / self._left_max_rpm) * 100.0
right_percentage = (right_target_rpm / self._right_max_rpm) * 100.0
#
# clip to +- 100%
left_percentage = max(min(left_percentage, 100.0), -100.0)
right_percentage = max(min(right_percentage, 100.0), -100.0)
#
# Add in a governor to cap forward motion when we're about
# to collide with something (but still backwards motion)
governor = 1.0
if self.distance < self.stop:
governor = 0.0
elif self.distance < self.close:
governor = (self.distance - self.stop) / (self.close - self.stop)
if right_percentage > 0:
right_percentage *= governor
if left_percentage > 0:
left_percentage *= governor
#
self._rightWheel.run(right_percentage)
self._leftWheel.run(left_percentage)
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()
from motor import Motor import time from servo import Servo servo = Servo(12) motor = Motor() velocity = 50 duration = 1 sleepTime = 2 motor.forward(velocity=velocity, duration=duration) servo.set_duty(80) motor.forward(velocity=velocity, duration=duration) time.sleep(sleepTime) servo.set_duty(70) motor.forward(velocity=velocity, duration=duration) time.sleep(sleepTime) servo.set_duty(90) motor.forward(velocity=velocity, duration=duration) time.sleep(sleepTime) servo.set_duty(77) servo.deinit_pwm()
from motor import Motor 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()
class IO_controller():
def __init__(self, trigPin, echoPin):
# Ultrasonic sensors pins
self.motor1 = Motor(12, 2, 3) #rechtsachter
self.motor2 = Motor(20, 4, 14) #rechtsvoor
self.motor3 = Motor(26, 15, 18) #linksachter
self.motor4 = Motor(16, 27, 17) #linksvoor
wiringPiSetupGpio()
print("IO controller initialised")
pinMode(trigPin, 1)
pinMode(echoPin, 1)
self.trigPin = trigPin
self.echoPin = echoPin
self.blackPin = 21
self.greyPin = 19
self.whitePin = 6
pinMode(self.blackPin, 0)
pinMode(self.greyPin, 0)
pinMode(self.whitePin, 0)
# Measure distance using ultrasonic sensor
def measure_distance(self):
# set Trigger to HIGH
digitalWrite(self.trigPin, 1)
# set Trigger after 0.01ms to LOW
time.sleep(0.00001)
digitalWrite(self.trigPin, 0)
StartTime = time.time()
StopTime = time.time()
# save StartTime
while digitalRead(self.echoPin) == 0:
StartTime = time.time()
# save time of arrival
while digitalRead(self.echoPin) == 1:
StopTime = time.time()
# time difference between start and arrival
TimeElapsed = StopTime - StartTime
# multiply with the sonic speed (34300 cm/s)
# and divide by 2, because there and back
distance = (TimeElapsed * 34300) / 2
#print("distance: " + distance + " cm")
return distance
# Detect lines and nodes
def detect_node(self):
print(digitalRead(self.whitePin))
if digitalRead(self.greyPin):
print("grey")
return "ground"
elif digitalRead(self.blackPin):
print("black")
return "line"
elif digitalRead(self.whitePin):
print("white")
return "node"
def forward(self, secs):
self.stop()
self.motor1.forward()
self.motor2.forward()
self.motor3.forward()
self.motor4.forward()
time.sleep(secs)
self.stop()
def reverse(self, secs):
self.stop()
self.motor1.reverse()
self.motor2.reverse()
self.motor3.reverse()
self.motor4.reverse()
time.sleep(secs)
self.stop()
def right(self):
self.stop()
self.motor1.reverse()
self.motor2.reverse()
self.motor3.forward()
self.motor4.forward()
time.sleep(2)
self.stop()
def left(self):
self.stop()
self.motor1.forward()
self.motor2.forward()
self.motor3.reverse()
self.motor4.reverse()
time.sleep(1.4)
self.stop()
def stop(self):
self.motor1.stop()
self.motor2.stop()
self.motor3.stop()
self.motor4.stop()
time.sleep(0.5)
