Ejemplo n.º 1
0
def move(direction):
    # Choose the direction of the request
    if direction == 'camleft':
        video_dir.move_decrease_x()
    elif direction == 'camright':
        video_dir.move_increase_x()
    elif direction == 'camup':
        video_dir.move_increase_y()
    elif direction == 'camdown':
        video_dir.move_decrease_y()
    elif direction == 'camhome':
        video_dir.home_x_y()
    elif direction == 'record':
        subprocess.call(['motion'], shell=True)
    elif direction == 'stoprecord':
        subprocess.call(['./stop.sh'], shell=True)
    elif direction == 'forward':
        motor.forward()
    elif direction == 'reverse':
        motor.backward()
    elif direction == 'left':
        car_dir.turn_left()
    elif direction == 'right':
        car_dir.turn_right()
    elif direction == 'stop':
        motor.ctrl(0)
    elif direction == 'home':
        car_dir.home()
Ejemplo n.º 2
0
def move(direction):
    # Choose the direction of the request
    if direction == 'camleft':
        video_dir.move_decrease_x()
    elif direction == 'camright':
        video_dir.move_increase_x()
    elif direction == 'camup':
        video_dir.move_increase_y()
    elif direction == 'camdown':
        video_dir.move_decrease_y()
    elif direction == 'camhome':
        video_dir.home_x_y()
    elif direction == 'record':
        subprocess.call(['motion'], shell=True)
    elif direction == 'stoprecord':
        subprocess.call(['./stop.sh'], shell=True)
    elif direction == 'forward':
        motor.forward()
    elif direction == 'reverse':
        motor.backward()
    elif direction == 'left':
        car_dir.turn_left()
    elif direction == 'right':
        car_dir.turn_right()
    elif direction == 'stop':
        motor.ctrl(0)
    elif direction == 'home':
        car_dir.home()
Ejemplo n.º 3
0
def command(cmd=None):
    if cmd == STOP:
        motor.stop()
    elif cmd == FORWARD:
        motor.forward()
    elif cmd == BACKWARD:
        motor.backward()
    elif cmd == LEFT:
        motor.left()
    else:
        motor.right()
    response = "Moving {}".format(cmd.capitalize())
    return response, 200, {'Content-Type': 'text/plain'}
Ejemplo n.º 4
0
def change(changePin):
    changePin = int(changePin)
    if changePin == 1:
        motor.turnLeft()
    elif changePin == 2:
        motor.forward()
    elif changePin == 3:
        motor.turnRight()
    elif changePin == 4:
        motor.backward()
    elif changePin == 5:
        motor.stop()
    else:
        print("Wrong command")

    return render_template("index.html")
Ejemplo n.º 5
0
def command(cmd=None):
    if cmd == STOP:
        led.led_stop()
        motor.stop()
    elif cmd == FORWARD:
        led.led_off()
        motor.forward()
    elif cmd == BACKWARD:
        led.led_off()
        motor.backward()
    elif cmd == LEFT:
        led.led_left()
        motor.left()
    elif cmd == RIGHT:
        led.led_right()
        motor.right()
    return "Success", 200, {'Content-Type': 'text/plain'}
Ejemplo n.º 6
0
async def hello(websocket, path):
    while True:
        msg = await websocket.recv()
        print("< {}".format(msg))

        if msg == 'forward':
            motor.forward()

        if msg == 'stop':
            motor.stop()

        if msg == 'right':
            motor.right()

        if msg == 'left':
            motor.left()

        if msg == 'backward':
            motor.backward()
Ejemplo n.º 7
0
def choice(input,conn):
    t=5
    start = t
    inc = 0.5
    try:
	if input == 'f':
            while t>0:
                if(sensor.willFrontCrash()):
                    print('stopped after ' + str(start-t))
                    break
                motor.forward(inc)
                t = t-inc
	    print('Move forward')
    	elif input == 'b':
            while t>0:
                if(sensor.willBackCrash()):
                    print('stopped after ' + str(start-t))
                    break
                motor.backward(inc)
                t = t-inc
	    print('Move backward')
    	elif input == 'r':
            motor.turnRight(t)
	    print('turn right')
    	elif input == 'l':	
            motor.turnLeft(t)
	    print('turn left')
	elif input == 's':
            reading = sensor.getAllSensor()
            print('Read all sensors')
            print(reading)
            result = " ".join(reading)
            conn.send(result)
    except KeyboardInterrupt:
	    print('key board interrupted!')
    finally:
            print('done')
    conn.send("\n")
Ejemplo n.º 8
0
def drive(stops):
    # setup all devices and initialize values
    busnum = 1
    car_dir.setup(busnum=busnum)
    motor.setup(busnum=busnum)
    distance_detect.setup()
    car_dir.home()
    motor.setSpeed(30)

    video_capture = cv2.VideoCapture(-1)
    video_capture.set(3, 160)
    video_capture.set(4, 120)
    command = 0
    stopCount = 0
    stopFound = 0

    #drive until passed certain amount of stops
    while True:
        # capture video frames
        ret, frame = video_capture.read()

        # set color masking boundaries for red and mask image
        colorLower = np.array([0, 0, 100], dtype='uint8')
        colorUpper = np.array([50, 50, 255], dtype='uint8')
        colorMask = cv2.inRange(frame, colorLower, colorUpper)
        outMask = cv2.bitwise_and(frame, frame, mask=colorMask)

        # create mask image, convert to grayscale, and blur
        clonedImg = outMask.copy()
        clonedImg = cv2.cvtColor(clonedImg, cv2.COLOR_BGR2GRAY)
        clonedImg = cv2.GaussianBlur(clonedImg, (5, 5), 0)

        # show current image
        cv2.namedWindow('Gray color select', cv2.WINDOW_NORMAL)
        cv2.imshow('Gray color select', clonedImg)

        # calculate circles within image
        circles = cv2.HoughCircles(clonedImg,
                                   cv2.cv.CV_HOUGH_GRADIENT,
                                   1,
                                   20,
                                   param1=50,
                                   param2=30,
                                   minRadius=15,
                                   maxRadius=100)

        # if a circle was detected
        if circles != None:
            # if this is first time encountering this stop increase stop count
            if stopFound == 0:
                stopCount += 1
                stopFound = 1

            # map out circles for display
            circles = np.uint16(np.around(circles))

            for i in circles[0, :]:
                cv2.circle(clonedImg, (i[0], i[1]), i[2], (0, 255, 0), 2)
                cv2.circle(clonedImg, (i[0], i[1]), 2, (0, 255, 0), 3)
        elif (cv2.countNonZero(clonedImg) == 0):
            stopFound = 0

        # display camera feed and circles
        cv2.namedWindow('Circles', cv2.WINDOW_NORMAL)
        cv2.imshow('Circles', clonedImg)

        # crop the image
        crop_img = frame[60:120, 0:160]

        # convert to grayscale
        gray = cv2.cvtColor(crop_img, cv2.COLOR_BGR2GRAY)

        # gaussian blur
        blur = cv2.GaussianBlur(gray, (5, 5), 0)

        # color thresholding
        ret, thresh = cv2.threshold(blur, 60, 255, cv2.THRESH_BINARY_INV)

        # find the contours of the frame
        contours, hierarchy = cv2.findContours(thresh.copy(), 1,
                                               cv2.CHAIN_APPROX_NONE)

        # detect distance to closes object
        dis = distance_detect.checkdis()
        print "distance: ", dis

        # find the biggest contour (if detected)
        if len(contours) > 0 and dis >= 15:
            c = max(contours, key=cv2.contourArea)
            M = cv2.moments(c)

            if M['m00'] != 0:
                cx = int(M['m10'] / M['m00'])
                cy = int(M['m01'] / M['m00'])

            cv2.line(crop_img, (cx, 0), (cx, 720), (255, 0, 0), 1)
            cv2.line(crop_img, (0, cy), (1280, cy), (255, 0, 0), 1)
            cv2.drawContours(crop_img, contours, -1, (0, 255, 0), 1)

            if cx >= 120:
                print "Turn Right ", cx
                car_dir.turn_right()
                motor.forward()

            if cx < 120 and cx > 50:
                print "On Track! ", cx
                car_dir.home()
                motor.forward()

            if cx <= 50:
                print "Turn Left! ", cx
                car_dir.turn_left()
                motor.forward()

        else:
            if dis < 15:
                print "something blocking me"
                car_dir.home()
                motor.stop()
            else:
                print "I don't see the line"
                car_dir.home()
                motor.backward()

        # display the resulting frame
        cv2.namedWindow('frame', cv2.WINDOW_NORMAL)
        cv2.imshow('frame', crop_img)

        # exit condition after passing certain amount of stops or 'q' is pressed
        if stopCount == stops or (cv2.waitKey(1) & 0xFF == ord('q')):
            # clean up
            motor.stop()
            distance_detect.cleanup()
            cv2.destroyAllWindows()
            break
Ejemplo n.º 9
0
	# one, which means it is suspended before the connection comes.
	tcpCliSock, addr = tcpSerSock.accept() 
	print('...connected from :', addr)     # Print the IP address of the client connected with the server.

	while True:
		data = ''
		data = tcpCliSock.recv(BUFSIZ)    # Receive data sent from the client. 
		# Analyze the command received and control the car accordingly.
		if not data:
			break
		if data == ctrl_cmd[0]:
			print('motor moving forward')
			motor.forward()
		elif data == ctrl_cmd[1]:
			print('recv backward cmd')
			motor.backward()
		elif data == ctrl_cmd[2]:
			print('recv left cmd')
			car_dir.turn_left()
		elif data == ctrl_cmd[3]:
			print('recv right cmd')
			car_dir.turn_right()
		elif data == ctrl_cmd[6]:
			print('recv home cmd')
			car_dir.home()
		elif data == ctrl_cmd[4]:
			print('recv stop cmd')
			motor.ctrl(0)
		elif data == ctrl_cmd[5]:
			print('read cpu temp...')
			temp = cpu_temp.read() # from RPi.GPIO
Ejemplo n.º 10
0
def backward():
	motor.backward()
Ejemplo n.º 11
0
    # http://ozzmaker.com/2013/04/18/success-with-a-balancing-robot-using-a-raspberry-pi/
    accAngX = (math.atan2(rate_accX, rate_accY) + M_PI) * RAD_TO_DEG
    CFangleX = K * (CFangleX + rate_gyroX * time_diff) + (1 - K) * accAngX

    # http://blog.bitify.co.uk/2013/11/reading-data-from-mpu-6050-on-raspberry.html
    accAngX1 = get_x_rotation(rate_accX, rate_accY, rate_accX)
    CFangleX1 = (K * (CFangleX1 + rate_gyroX * time_diff) + (1 - K) * accAngX1)

    # Followed the Second example because it gives resonable pid reading
    pid = int(p.update(CFangleX1))  #Add the sampling time in calculating PID
    speed = pid
    #print speed

    if (pid > 0):
        print "forward"
        MOTOR.forward(speed)
    elif (pid < 0):
        print "backward"
        MOTOR.backward(abs(speed))
    else:
        print "stop"
        #MOTOR.forward(speed)
        #MOTOR.backward( abs(speed) )
        MOTOR.stop(
        )  #Copy Paste forward and backward code here instead of stop()

    #print(j)
    #print "{0:.2f} {1:.2f} {2:.2f} {3:.2f} | {4:.2f} {5:.2f} | {6:.2f} | {7:.2f} ".format( gyroAngleX, gyroAngleY , accAngX, CFangleX, accAngX1, CFangleX1, pid, speed)
    #print "{0:.2f} {1:.2f} {2:.2f}".format( gyroAngleX, accAngX, CFangleX)
#print "{0:.2f} {1:.2f}".format( sensor.read_pitch(), sensor.read_roll())
Ejemplo n.º 12
0
def motor_backward(request):
	motor.backward()
	return HttpResponse("Motor backward")
Ejemplo n.º 13
0
def CarController(socket):
    while True:
        print 'Waiting for connection to car controller service...'
        # Waiting for connection. Once receiving a connection, the function accept() returns a separate
        # client socket for the subsequent communication. By default, the function accept() is a blocking
        # one, which means it is suspended before the connection comes.
        clientSocket, addr = socket.accept()
        print '...connected to car controller service:', addr  # Print the IP address of the client connected with the server.

        lastCmd = ''

        while True:
            msgs = ''
            recdata = clientSocket.recv(BUFSIZ)
            # Receive data sent from the client.
            # Analyze the command received and control the car accordingly.
            msgs = recdata.split(';')
            #print("Received", len(msgs), "new messages")
            for data in msgs:
                if not data:
                    break

                if lastCmd == data:
                    print("Last Command:", lastCmd, "Current Data:", data,
                          "Ignoring")
                    break

                if data == ctrl_cmd[0]:
                    print 'motor moving forward'
                    motor.forward()
                elif data == ctrl_cmd[1]:
                    print 'recv backward cmd'
                    motor.backward()
                elif data == ctrl_cmd[2]:
                    print 'recv left cmd'
                    car_dir.turn_left()
                elif data == ctrl_cmd[3]:
                    print 'recv right cmd'
                    car_dir.turn_right()
                elif data == ctrl_cmd[6]:
                    print 'recv home cmd'
                    car_dir.home()
                elif data == ctrl_cmd[4]:
                    print 'recv stop cmd'
                    motor.ctrl(0)
                elif data == ctrl_cmd[5]:
                    print 'read cpu temp...'
                    temp = cpu_temp.read()
                    tcpCliSock.send('[%s] %0.2f' % (ctime(), temp))
                elif data[0:5] == 'speed':
                    #print data
                    numLen = len(data) - len('speed')
                    if numLen == 1 or numLen == 2 or numLen == 3:
                        tmp = data[-numLen:]
                        #print 'tmp(str) = %s' % tmp
                        spd = int(tmp)
                        #print 'spd(int) = %d' % spd
                        if spd < 24:
                            spd = 24
                        motor.setSpeed(spd)
                elif data[0:8] == 'network=':
                    print 'network =', data
                    spd = data.split('=')[1]
                    try:
                        spd = int(spd)
                        os.system('sudo tc qdisc del dev wlan0 root')
                        os.system(
                            'sudo tc qdisc add dev wlan0 root netem delay {0}ms'
                            .format(spd))
                    except:
                        print 'ERROR , speed =', spd
                elif data[0:7] == 'offset=':
                    print 'offset called, data = ', data
                    offset = int(data[7:]) + 28
                    car_dir.calibrate(offset)
                elif data[0:8] == 'forward=':
                    #print 'data =', data
                    spd = data.split('=')[1]
                    try:
                        spd = int(spd)
                        motor.setSpeed(spd)
                        motor.forward()
                    except:
                        print 'Error speed =', spd
                elif data[0:9] == 'backward=':
                    #print 'data =', data
                    spd = data.split('=')[1]
                    try:
                        spd = int(spd)
                        motor.setSpeed(spd)
                        motor.backward()
                    except:
                        print 'ERROR , speed =', spd

                else:
                    print 'Command Error! Cannot recognize command: ' + data
Ejemplo n.º 14
0
            face_bool_count = 6  # Don't let the variable blow up.
            # Check if the robot is pointing right at the person's face.
            centered_face_bool, xShift_face, yShift_face = blob.check_centered(
                point)
            straighten_bool = blob.check_straighten(cameraPos)
            print('centered', centered_face_bool, 'straighten',
                  straighten_bool)
            print('cameraPos', cameraPos)
            if not centered_face_bool or not straighten_bool:  # If the robot is not pointing at the person's face,
                # straighten up.
                cameraPos = blob.straighten_up(xShift_face, yShift_face, 0, adjust_speed * 4,\
                                                cameraPos)
            else:
                # Visual cue that the robot is about to set the face's neutral expression.
                if not neutralBool:
                    motor.backward(speed)
                    time.sleep(.3)
                    motor.forward(speed)
                    time.sleep(.3)
                    motor.brake()
                    time.sleep(.5)
                # Set neutral expression if face is properly aligned.
                neutralBool, neutralFeaturesUpper, neutralFeaturesLower \
                             =face_op.set_neutral(feat, newFeaturesUpper, newFeaturesLower, neutralBool,tol)

        # Increase size of frame for viewing.
        big_frame = cv2.resize(small_frame, (0, 0),
                               fx=scaleUp * 1 / scaleFactor,
                               fy=scaleUp * 1 / scaleFactor)

        # Show text on video.
    def run(self):

        while True:
            print 'Waiting for connection...'

            tcpCliSock, addr = tcpSerSock.accept()
            print '...connected from :', addr

            while True:
                data = ''
                data = tcpCliSock.recv(BUFSIZ)
                if not data:
                    break
                if data == ctrl_cmd[0]:
                    print 'motor moving forward'
                    motor.forward()
                elif data == ctrl_cmd[1]:
                    print 'recv backward cmd'
                    motor.backward()
                elif data == ctrl_cmd[2]:
                    print 'recv left cmd'
                    car_dir.turn_left()
                elif data == ctrl_cmd[3]:
                    print 'recv right cmd'
                    car_dir.turn_right()
                elif data == ctrl_cmd[6]:
                    print 'recv home cmd'
                    car_dir.home()
                elif data == ctrl_cmd[4]:
                    print 'recv stop cmd'
                    motor.ctrl(0)
                elif data == ctrl_cmd[5]:
                    print 'read cpu temp...'
                    temp = cpu_temp.read()
                    tcpCliSock.send('[%s] %0.2f' % (ctime(), temp))
                elif data == ctrl_cmd[8]:
                    print 'recv x+ cmd'
                    video_dir.move_increase_x()
                elif data == ctrl_cmd[9]:
                    print 'recv x- cmd'
                    video_dir.move_decrease_x()
                elif data == ctrl_cmd[10]:
                    print 'recv y+ cmd'
                    video_dir.move_increase_y()
                elif data == ctrl_cmd[11]:
                    print 'recv y- cmd'
                    video_dir.move_decrease_y()
                elif data == ctrl_cmd[12]:
                    print 'home_x_y'
                    video_dir.home_x_y()
                elif data[0:5] == 'speed':
                    print data
                    numLen = len(data) - len('speed')
                    if numLen == 1 or numLen == 2 or numLen == 3:
                        tmp = data[-numLen:]
                        print 'tmp(str) = %s' % tmp
                        spd = int(tmp)
                        print 'spd(int) = %d' % spd
                        if spd < 24:
                            spd = 24
                        motor.setSpeed(spd)
                elif data[0:5] == 'turn=':
                    print 'data =', data
                    angle = data.split('=')[1]
                    try:
                        angle = int(angle)
                        car_dir.turn(angle)
                    except:
                        print 'Error: angle =', angle
                elif data[0:8] == 'forward=':
                    print 'data =', data
                    spd = data[8:]
                    try:
                        spd = int(spd)
                        motor.forward(spd)
                    except:
                        print 'Error speed =', spd
                elif data[0:9] == 'backward=':
                    print 'data =', data
                    spd = data.split('=')[1]
                    try:
                        spd = int(spd)
                        motor.backward(spd)
                    except:
                        print 'ERROR, speed =', spd

                else:
                    print 'Command Error! Cannot recognize command: ' + data

        tcpSerSock.close()
Ejemplo n.º 16
0
    raw_accX = sensor.read_raw_accel_x()
    raw_accY = sensor.read_raw_accel_y()
    raw_accZ = sensor.read_raw_accel_z()
    
    # Accelerometer value Degree Per Second / Scalled Data
    rate_accX = sensor.read_scaled_accel_x()
    rate_accY = sensor.read_scaled_accel_y()
    rate_accZ = sensor.read_scaled_accel_z()
    
    # http://ozzmaker.com/2013/04/18/success-with-a-balancing-robot-using-a-raspberry-pi/
    accAngX = ( math.atan2(rate_accX, rate_accY) + M_PI ) * RAD_TO_DEG
    CFangleX = K * ( CFangleX + rate_gyroX * time_diff) + (1 - K) * accAngX
    
    # http://blog.bitify.co.uk/2013/11/reading-data-from-mpu-6050-on-raspberry.html 
    accAngX1 = get_x_rotation(rate_accX, rate_accY, rate_accX)
    CFangleX1 = ( K * ( CFangleX1 + rate_gyroX * time_diff) + (1 - K) * accAngX1 )
    
    # Followed the Second example because it gives resonable pid reading
    pid = int(p.update(CFangleX1))
    speed = pid * 30

    if(pid > 0):
        MOTOR.forward(speed)
    elif(pid < 0):
        MOTOR.backward( abs(speed) )
    else:
	MOTOR.stop()

    print "{0:.2f} {1:.2f} {2:.2f} {3:.2f} | {4:.2f} {5:.2f} | {6:.2f} | {7:.2f} ".format( gyroAngleX, gyroAngleY , accAngX, CFangleX, accAngX1, CFangleX1, pid, speed)
    #print "{0:.2f} {1:.2f}".format( sensor.read_pitch(), sensor.read_roll())
Ejemplo n.º 17
0
         ENBset(100)
 elif str == 'keycode':
     arrow = int(datalist[0][1])
     if arrow == 38:
         print("Go Forward")
         mtr.forward()
         time.sleep(0.05)
         mtr.stop()
     elif arrow == 37:
         print("Turn Left")
         mtr.left()
         time.sleep(0.05)
         mtr.stop()
     elif arrow == 40:
         print("Go Backward")
         mtr.backward()
         time.sleep(0.05)
         mtr.stop()
     elif arrow == 39:
         print("Turn Right")
         mtr.right()
         time.sleep(0.05)
         mtr.stop()
 
 ## Control Arm/Camera Servo Motor
 elif str == 'range':
     servoNum = datalist[1][1]
     angle = int(datalist[0][1])
     if servoNum == 0: ## Reset Servo
         Servo.XiaoRGEEK_ReSetServo()
     Servo.XiaoRGEEK_SetServoAngle(servoNum, angle)
def Localization():
    global locationIndex
    #Peforming Localization Multi-Step Process
    MapDimensions = BitMap.shape
    indices = [i for i in range(MapDimensions[0] + 1)]
    indices[MapDimensions[0]] = -1
    totalReps = 3
    marginForBitError = totalReps - 1
    index = 0  #Only for readings array
    bitReading = np.zeros(totalReps)
    readings = np.zeros((totalReps, 17))

    while (marginForBitError >= 0):
        readings[index] = np.copy(Turn())
        print readings[index]
        ComparedBitValues = np.zeros(MapDimensions[0])
        bitReading[index] = bitMapRowGenerator(readings[index])

        counter = 0
        i = indices[counter]
        while (i != -1):
            if (i < MapDimensions[0]):
                ComparedBitValues[i] = 17 - (
                    bin(int(bitReading[index]) ^ int(BitMap[i])).count('1'))
            counter = counter + 1
            i = indices[counter]
        print ComparedBitValues

        if (marginForBitError > 0):
            counter = 0
            localmax = np.max(ComparedBitValues)
            for i in range(0, MapDimensions[0]):
                if (ComparedBitValues[i] + marginForBitError >= localmax):
                    indices[counter] = i + 1
                    counter = counter + 1
            indices[counter] = -1
            time.sleep(0.5)
            motor.isObstacle = 0
            motor.forward(0.75)
            time.sleep(0.5)
        else:
            time.sleep(0.5)
            motor.isObstacle = 0
            motor.backward(1.5)
            time.sleep(0.5)

        marginForBitError = marginForBitError - 1
        index = index + 1

    sI = LocationArray[locationIndex].initialIndex
    fI = LocationArray[locationIndex].finalIndex
    count = 0
    for i in range(sI, fI + 1):
        if (ComparedBitValues[i] >= 14):
            count += 1
    if (count == 0):
        newIndex = -1
        for x in range(1, 11):
            if (sI - x >= 0 and ComparedBitValues[sI - x] >= 14):
                newIndex = sI - x
                break
            if (fI + x >= 0 and ComparedBitValues[fI + x] >= 14):
                newIndex = fI + x
                break
        if (newIndex < 0):
            newIndex = np.max(ComparedBitValues)

        locationIndex = findLocationIndex(newIndex - 2)
def process_request(data):
    # Analyze the command received and control the car accordingly.
    if not data:
        return "cmd not understood"
    if data == ctrl_cmd[0]:
        print('motor moving forward')
        motor.forward()
    elif data == ctrl_cmd[1]:
        print('recv backward cmd')
        motor.backward()
    elif data == ctrl_cmd[2]:
        print('recv left cmd')
        car_dir.turn_left()
    elif data == ctrl_cmd[3]:
        print('recv right cmd')
        car_dir.turn_right()
    elif data == ctrl_cmd[6]:
        print('recv home cmd')
        car_dir.home()
    elif data == ctrl_cmd[4]:
        print('recv stop cmd')
        motor.ctrl(0)
    elif data == ctrl_cmd[5]:
        print('read cpu temp...')
        temp = cpu_temp.read()
        tcpCliSock.send('[%s] %0.2f' % (ctime(), temp))
    elif data == ctrl_cmd[8]:
        print('recv x+ cmd')
        video_dir.move_increase_x()
    elif data == ctrl_cmd[9]:
        print('recv x- cmd')
        video_dir.move_decrease_x()
    elif data == ctrl_cmd[10]:
        print('recv y+ cmd')
        video_dir.move_increase_y()
    elif data == ctrl_cmd[11]:
        print('recv y- cmd')
        video_dir.move_decrease_y()
    elif data == ctrl_cmd[12]:
        print('home_x_y')
        video_dir.home_x_y()
    elif data[0:5] == 'speed':     # Change the speed
        print(data)
        numLen = len(data) - len('speed')
        if numLen == 1 or numLen == 2 or numLen == 3:
            tmp = data[-numLen:]
            print('tmp(str) = %s' % tmp)
            spd = int(tmp)
            print('spd(int) = %d' % spd)
            if spd < 24:
                spd = 24
            motor.setSpeed(spd)
    elif data[0:5] == 'turn=':  # Turning Angle
        print('data =', data)
        angle = data.split('=')[1]
        try:
            angle = int(angle)
            car_dir.turn(angle)
        except:
            print('Error: angle =', angle)
    elif data[0:8] == 'forward=':
        print('data =', data)
        spd = data[8:]
        try:
            spd = int(spd)
            motor.forward(spd)
        except:
            print('Error speed =', spd)
    elif data[0:9] == 'backward=':
        print('data =', data)
        spd = data.split('=')[1]
        try:
            spd = int(spd)
            motor.backward(spd)
        except:
            print('ERROR, speed =', spd)
    else:
        print('Command Error! Cannot recognize command: ' + data)
Ejemplo n.º 20
0
def gen():
    """Video streaming generator function."""
    cap = cv2.VideoCapture(0)
    faceDetect = cv2.CascadeClassifier('haarcascade_frontalface_default.xml')
    rec = cv2.face.LBPHFaceRecognizer_create()
    rec.read("recognizer/training_data.yml")
    i = 0
    fourcc = cv2.VideoWriter_fourcc(*'XVID')
    out = cv2.VideoWriter('output' + str(i) + '.avi', fourcc, 10.0, (640, 480))
    engine = pyttsx3.init()
    engine.setProperty(
        'voice',
        "HKEY_LOCAL_MACHINE\SOFTWARE\Microsoft\Speech\Voices\Tokens\TTS_MS_EN-US_ZIRA_11.0"
    )  #set voice type
    engine.setProperty('rate', 100)  ### 10 adalah kecepatan baca
    #cap.set(cv2.CAP_PROP_FRAME_WIDTH, 160)
    #cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 120)
    # Read until video is completed
    while (cap.isOpened()):
        ret, frame = cap.read()  # import image
        if not ret:  #if vid finish repeat
            cap = cv2.VideoCapture(0)
            continue
        if ret:  # if there is a frame continue with code
            faceDetect = cv2.CascadeClassifier(
                'haarcascade_frontalface_default.xml')
            gray = cv2.cvtColor(frame,
                                cv2.COLOR_BGR2GRAY)  # converts image to gray
            faces = faceDetect.detectMultiScale(gray, 1.3, 5)
            cv2.putText(frame, str(datetime.now()), (10, 50),
                        cv2.FONT_HERSHEY_SIMPLEX, 1.0, (0, 0, 255), 2,
                        cv2.LINE_AA)
            for (x, y, w, h) in faces:
                cv2.rectangle(frame, (x, y), (x + w, y + h), (0, 0, 255),
                              4)  #cv2.imshow("countours", image)
                id, conf = rec.predict(gray[y:y + h, x:x + w])
                if (id == 1):
                    text = "ILHAM"
                    engine.say(text)
                    engine.runAndWait()
                else:
                    text = "UNKNOWN"
                    engine.say(text)
                    engine.runAndWait()
                cv2.putText(frame, text, (10, 80), cv2.FONT_HERSHEY_SIMPLEX,
                            1.0, (0, 0, 255), 2, cv2.LINE_AA)
                if os.path.exists("output" + str(i) + ".avi"):
                    i += 1
                out.write(frame)
                full = w + h
                tengah = x + (w / 2)
                print("posisi x :", str(tengah))
                print("full" + str(full))
                if tengah < 270:
                    motor.turnLeft()
                elif tengah > 370:
                    motor.turnRight()
                else:
                    motor.stop()

                if full > 470:
                    motor.backward()
                elif full < 430:
                    motor.forward()
                else:
                    motor.stop()

        frame = cv2.imencode('.jpg', frame)[1].tobytes()
        yield (b'--frame\r\n'
               b'Content-Type: image/jpeg\r\n\r\n' + frame + b'\r\n')
        #time.sleep(0.1)
        key = cv2.waitKey(20)
        if key == 27:
            break
    motor.cleanup()
Ejemplo n.º 21
0
def on_message(ws, data):
    print data
    # Analyze the command received and control the car accordingly.
    #if not data:

    if data == ctrl_cmd[0]:
        print 'motor moving forward'
        motor.forward()
    elif data == ctrl_cmd[1]:
        print 'recv backward cmd'
        motor.backward()
    elif data == ctrl_cmd[2]:
        print 'recv left cmd'
        car_dir.turn_left()
    elif data == ctrl_cmd[3]:
        print 'recv right cmd'
        car_dir.turn_right()
    elif data == ctrl_cmd[6]:
        print 'recv home cmd'
        car_dir.home()
    elif data == ctrl_cmd[4]:
        print 'recv stop cmd'
        motor.ctrl(0)
    elif data == ctrl_cmd[5]:
        print 'read cpu temp...'
        temp = cpu_temp.read()
        tcpCliSock.send('[%s] %0.2f' % (ctime(), temp))
    elif data == ctrl_cmd[8]:
        print 'recv x+ cmd'
        video_dir.move_increase_x()
    elif data == ctrl_cmd[9]:
        print 'recv x- cmd'
        video_dir.move_decrease_x()
    elif data == ctrl_cmd[10]:
        print 'recv y+ cmd'
        video_dir.move_increase_y()
    elif data == ctrl_cmd[11]:
        print 'recv y- cmd'
        video_dir.move_decrease_y()
    elif data == ctrl_cmd[12]:
        print 'home_x_y'
        video_dir.home_x_y()
    elif data[0:12] == ctrl_cmd[13]:     # Change the speed
        print data
        #numLen = len(data) - len('speed')
        #if numLen == 1 or numLen == 2 or numLen == 3:
        #    tmp = data[-numLen:]
        #    print 'tmp(str) = %s' % tmp
        #    spd = int(tmp)
        #    print 'spd(int) = %d' % spd
        #    if spd < 24:
        #        spd = 24
        motor.setSpeed(30)
    elif data[0:5] == 'turn=':	#Turning Angle
        print 'data =', data
        angle = data.split('=')[1]
        try:
            angle = int(angle)
            car_dir.turn(angle)
        except:
            print 'Error: angle =', angle
    elif data[0:8] == 'forward=':
        print 'data =', data
        spd = 30
        try:
            spd = int(spd)
            motor.forward(spd)
        except:
            print 'Error speed =', spd
    elif data[0:9] == 'backward=':
        print 'data =', data
        spd = data.split('=')[1]
        try:
            spd = int(spd)
            motor.backward(spd)
        except:
            print 'ERROR, speed =', spd

    else:
        print 'Command Error! Cannot recognize command: ' + data
Ejemplo n.º 22
0
 def backward(self, speed=None):
     if speed is None:
         motor.backward()
     else:
         motor.backwardWithSpeed(int(speed))
     return self.ack()
Ejemplo n.º 23
0
def run_server(key):
    #!/usr/bin/env python
    import RPi.GPIO as GPIO
    import video_dir
    import car_dir
    import motor
    from time import ctime  # Import necessary modules

    ctrl_cmd = [
        'forward', 'backward', 'left', 'right', 'stop', 'read cpu_temp',
        'home', 'distance', 'x+', 'x-', 'y+', 'y-', 'xy_home'
    ]

    busnum = 1  # Edit busnum to 0, if you uses Raspberry Pi 1 or 0

    HOST = ''  # The variable of HOST is null, so the function bind( ) can be bound to all valid addresses.
    PORT = 21567
    BUFSIZ = 1024  # Size of the buffer
    ADDR = (HOST, PORT)

    tcpSerSock = socket(AF_INET, SOCK_STREAM)  # Create a socket.
    tcpSerSock.bind(ADDR)  # Bind the IP address and port number of the server.
    tcpSerSock.listen(
        5
    )  # The parameter of listen() defines the number of connections permitted at one time. Once the
    # connections are full, others will be rejected.

    #video_dir.setup(busnum=busnum)
    #car_dir.setup(busnum=busnum)
    #motor.setup(busnum=busnum)     # Initialize the Raspberry Pi GPIO connected to the DC motor.
    #video_dir.home_x_y()
    #car_dir.home()

    while True:
        print 'Waiting for connection...'
        # Waiting for connection. Once receiving a connection, the function accept() returns a separate
        # client socket for the subsequent communication. By default, the function accept() is a blocking
        # one, which means it is suspended before the connection comes.
        tcpCliSock, addr = tcpSerSock.accept()
        print '...connected from :', addr  # Print the IP address of the client connected with the server.

        while True:
            try:
                data = ''
                data = tcpCliSock.recv(
                    BUFSIZ)  # Receive data sent from the client.
                print "\nEncrypted command recieved from client = ,", eval(
                    data)[2]
                data = crypto.AES_decrypt(eval(data), key)
            except:
                print "INCOMPLETE PACKET ERROR - try to input the command again"
            # Analyze the command received and control the car accordingly.

            if not data:
                break
            if data == ctrl_cmd[0]:
                print 'motor moving forward'
                motor.forward()
            elif data == ctrl_cmd[1]:
                print 'recv backward cmd'
                motor.backward()
            elif data == ctrl_cmd[2]:
                print 'recv left cmd'
                car_dir.turn_left()
            elif data == ctrl_cmd[3]:
                print 'recv right cmd'
                car_dir.turn_right()
            elif data == ctrl_cmd[6]:
                print 'recv home cmd'
                car_dir.home()
            elif data == ctrl_cmd[4]:
                print 'recv stop cmd'
                motor.ctrl(0)
            elif data == ctrl_cmd[5]:
                print 'read cpu temp...'
                temp = cpu_temp.read()
                tcpCliSock.send('[%s] %0.2f' % (ctime(), temp))
            elif data == ctrl_cmd[8]:
                print 'recv x+ cmd'
                video_dir.move_increase_x()
            elif data == ctrl_cmd[9]:
                print 'recv x- cmd'
                video_dir.move_decrease_x()
            elif data == ctrl_cmd[10]:
                print 'recv y+ cmd'
                video_dir.move_increase_y()
            elif data == ctrl_cmd[11]:
                print 'recv y- cmd'
                video_dir.move_decrease_y()
            elif data == ctrl_cmd[12]:
                print 'home_x_y'
                video_dir.home_x_y()
            elif data[0:5] == 'speed':  # Change the speed
                print data
                numLen = len(data) - len('speed')
                if numLen == 1 or numLen == 2 or numLen == 3:
                    tmp = data[-numLen:]
                    print 'tmp(str) = %s' % tmp
                    spd = int(tmp)
                    print 'spd(int) = %d' % spd
                    if spd < 24:
                        spd = 24
                    motor.setSpeed(spd)
            elif data[0:5] == 'turn=':  #Turning Angle
                print 'data =', data
                angle = data.split('=')[1]
                try:
                    angle = int(angle)
                    car_dir.turn(angle)
                except:
                    print 'Error: angle =', angle
            elif data[0:8] == 'forward=':
                print 'data =', data
                spd = data[8:]
                try:
                    spd = int(spd)
                    motor.forward(spd)
                except:
                    print 'Error speed =', spd

            elif data[0:9] == 'backward=':
                print 'data =', data
                spd = data.split('=')[1]
                try:
                    spd = int(spd)
                    motor.backward(spd)
                except:
                    print 'ERROR, speed =', spd

            else:
                #print 'Command Error! Cannot recognize command: ' + data
                print "COMMAND ERROR - Unable to interpret recieved command"
    tcpSerSock.close()
Ejemplo n.º 24
0
        gyroAngleY += rate_gyroY * time_diff
        gyroAngleZ += rate_gyroZ * time_diff

        # Accelerometer Raw Value
        raw_accX = sensor.read_raw_accel_x()
        raw_accY = sensor.read_raw_accel_y()
        raw_accZ = sensor.read_raw_accel_z()
       
        # Accelerometer value Degree Per Second / Scalled Data
        rate_accX = sensor.read_scaled_accel_x()
        rate_accY = sensor.read_scaled_accel_y()
        rate_accZ = sensor.read_scaled_accel_z()
       
        # http://ozzmaker.com/2013/04/18/success-with-a-balancing-robot-using-a-raspberry-pi/
        accAngX = ( math.atan2(rate_accX, rate_accY) + M_PI ) * RAD_TO_DEG
        CFangleX = K * ( CFangleX + rate_gyroX * time_diff) + (1 - K) * accAngX
       
        # http://blog.bitify.co.uk/2013/11/reading-data-from-mpu-6050-on-raspberry.html
        accAngX1 = get_x_rotation(rate_accX, rate_accY, rate_accX)
        CFangleX1 = ( K * ( CFangleX1 + rate_gyroX * time_diff) + (1 - K) * accAngX1 )
       
        # Followed the Second example because it gives resonable pid reading
        pid = p.update(CFangleX1)
       
        if(pid > 0):
            MOTOR.forward(pid)
        else:
            MOTOR.backward( (pid*-1) )
       
        print "{0:.2f} {1:.2f} {2:.2f} {3:.2f} | {4:.2f} {5:.2f} | {6:.2f}".format( gyroAngleX, gyroAngleY , accAngX, CFangleX, accAngX1, CFangleX1, pid)
Ejemplo n.º 25
0
def server(
    value, collision
):  #get value as Direction and collision as Collision detection from Shared memory
    pre_dir = 'home'
    x = 0
    flag = 0
    while True:
        sys.stdout.flush()
        print 'Waiting for connection...'
        # Waiting for connection. Once receiving a connection, the function accept() returns a separate
        # client socket for the subsequent communication. By default, the function accept() is a blocking
        # one, which means it is suspended before the connection comes.
        tcpCliSock, addr = tcpSerSock.accept()
        print '...connected from :', addr  # Print the IP address of the client connected with the server.

        while True:
            data = ''
            data = tcpCliSock.recv(BUFSIZ)
            # Receive data sent from the client.
            # Analyze the command received and control the car accordingly.
            print data
            if not data:
                break
            if x == 1:
                if flag < 5:
                    flag = flag + 1
                    continue  #if there is any collision, Do not receive data from client.If so, Get stacked!
            x = 0  #after refusing data from client, start receiving
            flag = 0
            if data == ctrl_cmd[0]:  #If Client send message "forward"
                if collision.value == 1:  #if there is obstacle in front of iCar
                    motor.ctrl(0)  #stop
                else:
                    motor.forward()  #Run the motors to forward
            elif data == ctrl_cmd[1]:  #If Client send message "backward"
                motor.backward()

            elif data == ctrl_cmd[2]:  #If Client send message "left"
                car_dir.turn_left()  #change car direction to Left

            elif data == ctrl_cmd[3]:  #If Client send message "right"
                car_dir.turn_right()  #change car direction to Right

            elif data == ctrl_cmd[6]:  #If Client send message "home"
                car_dir.home()  #Set car direction to center

            elif data == ctrl_cmd[4]:  #if Client send message "stop"
                motor.ctrl(0)  #Stop Motor running

            elif data == ctrl_cmd[
                    5]:  #If Client click auto button, send message "auto"
                #auto drive
                motor.setSpeed(44)  #Set motor speed with  optimized speed 44
                temp = value.value  #get Value from jeongwook.py that is information of Car Direction

                if collision.value != 0:  #If there is Collision
                    print 'Collision detected'
                    if collision.value == 1:  #Collision in front of iCar
                        print "Obstacle In Front"
                        motor.collision_auto(
                        )  #collision_auto function let iCar move backward
                        car_dir.turn_right()  #to avoid collision, turn right
                        motor.forward_auto(
                        )  #move only for 0.8second (forward_auto let iCar move for 0.2second)
                        motor.forward_auto()
                        motor.forward_auto()
                        motor.forward_auto()
                        car_dir.home()
                        motor.forward_auto()
                        motor.forward_auto()
                        pre_dir = 'left'  #if iCar cannot detect any lane, it SHOULD BE on left side, so makes iCar go left after avoiding
                    elif collision.value == 2:  #Collision on Left side
                        print "Obstacle is on Left"
                        motor.collision_auto()
                        car_dir.turn_right()  #to avoid collision, turn right
                        motor.forward_auto()
                        motor.forward_auto()
                        motor.forward_auto()
                        motor.forward_auto()
                        car_dir.home()
                        motor.forward_auto()
                        motor.forward_auto()
                        pre_dir = 'left'
                    elif collision.value == 3:  #go left
                        print "Obstacle is on Right"
                        motor.collision_auto()
                        car_dir.turn_left()  #to avoid collision, turn left
                        motor.forward_auto()
                        motor.forward_auto()
                        motor.forward_auto()
                        motor.forward_auto()
                        car_dir.home()
                        motor.forward_auto()
                        motor.forward_auto()
                        pre_dir = 'right'  #if iCar cannot detect any lane, it SHOULD BE on right side, so makes iCar go right after avoiding

                    collision.value = 0

                    x = 1  #set x = 1 to Not receiving data from client for a moment

                elif temp == 1:  #iCar is on Lane, Go with center direction
                    print 'Lane is on my way'
                    car_dir.home()
                    motor.forward_auto()  #move iCar for 0.2second with 44speed
                    #because of camera delay

                elif temp == 2:  #lane is located on left side
                    print 'Lane is on left side'
                    car_dir.turn_left()
                    motor.forward_auto()
                    pre_dir = 'left'

                elif temp == 3:  #lane is located on right side
                    print 'Lane is on right side'
                    car_dir.turn_right()
                    motor.setSpeed(44)
                    motor.forward_auto()
                    pre_dir = 'right'
                else:
                    if pre_dir == 'right':  #when No detection but predict that lane is on right side
                        print 'cannot find but go right'
                        car_dir.turn_right()
                        motor.setSpeed(44)
                        motor.forward_auto()

                    elif pre_dir == 'left':  #when No detection but predict that lane is on left side
                        print 'cannot find but go left'
                        car_dir.turn_left()
                        motor.forward_auto()

                    else:
                        print 'Cannot find a Lane'  #No detection with no prediction
                        car_dir.home()  #set center direction and stop
                        motor.backward()
                        time.sleep(0.6)
                        motor.ctrl(0)
                        time.sleep(1)

            elif data == ctrl_cmd[7]:  #move camera right
                video_dir.move_increase_x()
            elif data == ctrl_cmd[8]:  #move camera left
                video_dir.move_decrease_x()
            elif data == ctrl_cmd[9]:  #move camera up
                video_dir.move_increase_y()
            elif data == ctrl_cmd[10]:  #move camera down
                video_dir.move_decrease_y()

            elif data[0:5] == 'speed':  # Change the speed
                print data
                numLen = len(data) - len('speed')
                if numLen == 1 or numLen == 2 or numLen == 3:
                    tmp = data[-numLen:]
                    spd = int(tmp)
                    if spd < 24:
                        spd = 24
                    motor.setSpeed(spd)

            elif data[
                    0:
                    7] == 'leftPWM':  #If Client send message like ~PWM, it is for initialization or servo motor that control Car direction.
                numLen = len(data) - len('leftPWM')
                pwm = data[-numLen:]
                leftpwm = int(pwm)

                car_dir.change_left(leftpwm)
            elif data[0:7] == 'homePWM':
                numLen = len(data) - len('homePWM')
                pwm = data[-numLen:]
                homepwm = int(pwm)

                car_dir.change_home(homepwm)
            elif data[0:8] == 'rightPWM':
                numLen = len(data) - len('rightPWM')
                pwm = data[-numLen:]
                rightpwm = int(pwm)
                car_dir.change_right(rightpwm)

            else:
                print 'Command Error! Cannot recognize command: ' + data

    tcpSerSock.close()
Ejemplo n.º 26
0
        elif pred < 0.03 and pred > -0.03:
            data = ctrl_cmd[6]
        else:
            angle = int((pred / 2 + 0.5) * 170 + 35)
            data = "turn=" + str(angle)

        if lstop_detected:
            motor.forward()
            lstop_detected = False

        if data == ctrl_cmd[0]:
            print 'motor moving forward'
            motor.forward()
        elif data == ctrl_cmd[1]:
            print 'recv backward cmd'
            motor.backward()
        elif data == ctrl_cmd[2]:
            print 'recv left cmd'
            car_dir.turn_left()
        elif data == ctrl_cmd[3]:
            print 'recv right cmd'
            car_dir.turn_right()
        elif data == ctrl_cmd[6]:
            print 'recv home cmd'
            car_dir.home()
        elif data == ctrl_cmd[4]:
            print 'recv stop cmd'
            motor.ctrl(0)
        elif data == ctrl_cmd[5]:
            print 'read cpu temp...'
            temp = cpu_temp.read()
        elif data == ctrl_cmd[8]:
	# one, which means it is suspended before the connection comes.
	tcpCliSock, addr = tcpSerSock.accept() 
	print '...connected from :', addr     # Print the IP address of the client connected with the server.

	while True:
		data = ''
		data = tcpCliSock.recv(BUFSIZ)    # Receive data sent from the client. 
		# Analyze the command received and control the car accordingly.
		if not data:
			break
		if data == ctrl_cmd[0]:
			print 'motor moving forward'
			motor.forward()
		elif data == ctrl_cmd[1]:
			print 'recv backward cmd'
			motor.backward()
		elif data == ctrl_cmd[2]:
			print 'recv left cmd'
			car_dir.turn_left()
		elif data == ctrl_cmd[3]:
			print 'recv right cmd'
			car_dir.turn_right()
		elif data == ctrl_cmd[6]:
			print 'recv home cmd'
			car_dir.home()
		elif data == ctrl_cmd[4]:
			print 'recv stop cmd'
			motor.ctrl(0)
		elif data == ctrl_cmd[5]:
			print 'read cpu temp...'
			temp = cpu_temp.read()
Ejemplo n.º 28
0
joy = xbox.Joystick()

# Setup motors
GPIO.setmode(GPIO.BOARD)
motor.setup()

try:
    #Valid connect may require joystick input to occur
    print "Waiting for Joystick to connect"
    while not joy.connected():
        time.sleep(0.10)

    print('Connected')
    #Show misc inputs until Back button is pressed    
    while not joy.Back() and joy.connected():
        if joy.rightTrigger() > 0:
            motor.forward()
        elif joy.leftTrigger() > 0:
            motor.backward()
        elif joy.rightBumper():
            motor.right()
        elif joy.leftBumper():
            motor.left()
        else:
            motor.stop()

finally:
    #Always close out so that xboxdrv subprocess ends
    joy.close()
    print "Done."
Ejemplo n.º 29
0
			direction = total_data[1]

			if (forward == "3") :
				speed = 35
				motor.setSpeed(speed)
				motor.forward()

			elif (forward == "2") :
				speed = 25
				motor.setSpeed(speed)
				motor.forward()

			elif (forward =="1"):
				speed = 25
				motor.setSpeed(speed)
				motor.backward()

			elif (forward == "0") :
				speed = 30
				motor.setSpeed(speed)
				motor.backward()

			elif forward == "4":
				motor.stop()

			elif forward == "5":
				motor.stop()


				
			if direction == "0":
Ejemplo n.º 30
0
 def drive_backward(self):
     sfdriving.backward()
     return True
Ejemplo n.º 31
0
	while True:
		data = ''
		data = tcpCliSock.recv(BUFSIZ)    # Receive data sent from the client. 
		# Analyze the command received and control the car accordingly.
		if not data:
			break

		print('data : ' + str(data))

		elif data == ctrl_cmd[0]:
			
			motor.forward()
		elif data == ctrl_cmd[1]:
			
			motor.backward()
		elif data == ctrl_cmd[2]:
			
			car_dir.turn_left()
		elif data == ctrl_cmd[3]:
			
			car_dir.turn_right()
		elif data == ctrl_cmd[6]:
			
			car_dir.home()
		elif data == ctrl_cmd[4]:
			
			motor.ctrl(0)
		elif data == ctrl_cmd[5]:
			
			temp = cpu_temp.read()
Ejemplo n.º 32
0
def b():
    backward()
    return jsonify({"message":"Started moving backward"})