Esempio n. 1
0
def test():
    gpio.setmode(gpio.BCM)
    distance_sensor = DistanceSensor()
    motor = Motor(input1=22, input2=27, input3=17, input4=4)

    while True:
        motor.forward()
Esempio n. 2
0
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__()
Esempio n. 3
0
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);
Esempio n. 4
0
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)
Esempio n. 5
0
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()
Esempio n. 6
0
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()
Esempio n. 7
0
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()
Esempio n. 9
0
# 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)
Esempio n. 10
0
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()
Esempio n. 11
0
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()
Esempio n. 12
0
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()
Esempio n. 13
0
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:
Esempio n. 15
0
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)
Esempio n. 16
0
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()          
Esempio n. 17
0
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()
Esempio n. 18
0
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()
Esempio n. 19
0
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()
Esempio n. 20
0
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)